Copter: integrate AC_Loiter

includes param conversion

ArduCopter/Copter.h

@@ -68,6 +68,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>           // ArduCopter waypoint navigation library
+#include <AC_WPNav/AC_Loiter.h>
 #include <AC_WPNav/AC_Circle.h>          // circle navigation library
 #include <AP_Declination/AP_Declination.h>     // ArduPilot Mega Declination Helper Library
 #include <AP_Scheduler/AP_Scheduler.h>       // main loop scheduler
@@ -501,6 +502,7 @@ private:
     AC_AttitudeControl_t *attitude_control;
     AC_PosControl *pos_control;
     AC_WPNav *wp_nav;
+    AC_Loiter *loiter_nav;
 #if MODE_CIRCLE_ENABLED == ENABLED
     AC_Circle *circle_nav;
 #endif

ArduCopter/Parameters.cpp

@@ -578,6 +578,10 @@ const AP_Param::Info Copter::var_info[] = {
     // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp
     GOBJECTPTR(wp_nav, "WPNAV_",       AC_WPNav),
 
+    // @Group: LOIT_
+    // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp
+    GOBJECTPTR(loiter_nav, "LOIT_", AC_Loiter),
+
 #if MODE_CIRCLE_ENABLED == ENABLED
     // @Group: CIRCLE_
     // @Path: ../libraries/AC_WPNav/AC_Circle.cpp
@@ -1043,7 +1047,7 @@ const AP_Param::ConversionInfo conversion_table[] = {
     // battery
     { Parameters::k_param_fs_batt_voltage,    0,      AP_PARAM_INT8,  "BATT_FS_LOW_VOLT" },
     { Parameters::k_param_fs_batt_mah,        0,      AP_PARAM_INT8,  "BATT_FS_LOW_MAH" },
-    { Parameters::k_param_failsafe_battery_enabled,        0,      AP_PARAM_INT8,  "BATT_FS_LOW_ACT" },
+    { Parameters::k_param_failsafe_battery_enabled,0, AP_PARAM_INT8,  "BATT_FS_LOW_ACT" },
 };
 
 void Copter::load_parameters(void)
@@ -1134,6 +1138,12 @@ void Copter::convert_pid_parameters(void)
         { Parameters::k_param_throttle_min, 0, AP_PARAM_FLOAT, "MOT_SPIN_MIN" },
         { Parameters::k_param_throttle_mid, 0, AP_PARAM_FLOAT, "MOT_THST_HOVER" }
     };
+    const AP_Param::ConversionInfo loiter_conversion_info[] = {
+        { Parameters::k_param_wp_nav, 4, AP_PARAM_FLOAT, "LOIT_SPEED" },
+        { Parameters::k_param_wp_nav, 7, AP_PARAM_FLOAT, "LOIT_BRK_JERK" },
+        { Parameters::k_param_wp_nav, 8, AP_PARAM_FLOAT, "LOIT_ACC_MAX" },
+        { Parameters::k_param_wp_nav, 9, AP_PARAM_FLOAT, "LOIT_BRK_ACCEL" }
+    };
 
     // gains increase by 27% due to attitude controller's switch to use radians instead of centi-degrees
     // and motor libraries switch to accept inputs in -1 to +1 range instead of -4500 ~ +4500
@@ -1172,6 +1182,11 @@ void Copter::convert_pid_parameters(void)
     if (AP_Param::find_old_parameter(&rc_feel_rp_conversion_info, &rc_feel_rp_old)) {
         AP_Param::set_default_by_name(rc_feel_rp_conversion_info.new_name, (1.0f / (2.0f + rc_feel_rp_old.get() * 0.1f)));
     }
+    // convert loiter parameters
+    table_size = ARRAY_SIZE(loiter_conversion_info);
+    for (uint8_t i=0; i<table_size; i++) {
+        AP_Param::convert_old_parameter(&loiter_conversion_info[i], 1.0f);
+    }
 
     const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old,  Parameters::k_param_rc_2_old,
                                       Parameters::k_param_rc_3_old,  Parameters::k_param_rc_4_old,

ArduCopter/Parameters.h

@@ -196,7 +196,8 @@ public:
         k_param_wp_nav,
         k_param_attitude_control,
         k_param_pos_control,
-        k_param_circle_nav,     // 104
+        k_param_circle_nav,
+        k_param_loiter_nav,     // 105
 
         // 110: Telemetry control
         //

ArduCopter/mode.cpp

@@ -12,6 +12,7 @@ Copter::Mode::Mode(void) :
     g(copter.g),
     g2(copter.g2),
     wp_nav(copter.wp_nav),
+    loiter_nav(copter.loiter_nav),
     pos_control(copter.pos_control),
     inertial_nav(copter.inertial_nav),
     ahrs(copter.ahrs),
@@ -454,7 +455,7 @@ void Copter::Mode::land_run_horizontal_control()
 
     // relax loiter target if we might be landed
     if (ap.land_complete_maybe) {
-        wp_nav->loiter_soften_for_landing();
+        loiter_nav->soften_for_landing();
     }
 
     // process pilot inputs
@@ -472,7 +473,7 @@ void Copter::Mode::land_run_horizontal_control()
             update_simple_mode();
 
             // convert pilot input to lean angles
-            get_pilot_desired_lean_angles(target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
+            get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
 
             // record if pilot has overriden roll or pitch
             if (!is_zero(target_roll) || !is_zero(target_pitch)) {
@@ -503,13 +504,13 @@ void Copter::Mode::land_run_horizontal_control()
 #endif
 
     // process roll, pitch inputs
-    wp_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
+    loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
 
     // run loiter controller
-    wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+    loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
 
-    int32_t nav_roll  = wp_nav->get_roll();
-    int32_t nav_pitch = wp_nav->get_pitch();
+    int32_t nav_roll  = loiter_nav->get_roll();
+    int32_t nav_pitch = loiter_nav->get_pitch();
 
     if (g2.wp_navalt_min > 0) {
         // user has requested an altitude below which navigation

ArduCopter/mode.h

@@ -54,6 +54,7 @@ protected:
     Parameters &g;
     ParametersG2 &g2;
     AC_WPNav *&wp_nav;
+    AC_Loiter *&loiter_nav;
     AC_PosControl *&pos_control;
     AP_InertialNav &inertial_nav;
     AP_AHRS &ahrs;

ArduCopter/mode_auto.cpp

@@ -218,7 +218,7 @@ void Copter::ModeAuto::land_start()
 {
     // set target to stopping point
     Vector3f stopping_point;
-    wp_nav->get_loiter_stopping_point_xy(stopping_point);
+    loiter_nav->get_stopping_point_xy(stopping_point);
 
     // call location specific land start function
     land_start(stopping_point);
@@ -230,7 +230,7 @@ void Copter::ModeAuto::land_start(const Vector3f& destination)
     _mode = Auto_Land;
 
     // initialise loiter target destination
-    wp_nav->init_loiter_target(destination);
+    loiter_nav->init_target(destination);
 
     // initialise position and desired velocity
     if (!pos_control->is_active_z()) {
@@ -360,7 +360,7 @@ void Copter::ModeAuto::payload_place_start()
 {
     // set target to stopping point
     Vector3f stopping_point;
-    wp_nav->get_loiter_stopping_point_xy(stopping_point);
+    loiter_nav->get_stopping_point_xy(stopping_point);
 
     // call location specific place start function
     payload_place_start(stopping_point);
@@ -852,7 +852,7 @@ void Copter::ModeAuto::land_run()
     if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
         zero_throttle_and_relax_ac();
         // set target to current position
-        wp_nav->init_loiter_target();
+        loiter_nav->init_target();
         return;
     }
 
@@ -928,7 +928,7 @@ void Copter::ModeAuto::payload_place_start(const Vector3f& destination)
     nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
 
     // initialise loiter target destination
-    wp_nav->init_loiter_target(destination);
+    loiter_nav->init_target(destination);
 
     // initialise position and desired velocity
     if (!pos_control->is_active_z()) {
@@ -947,7 +947,7 @@ void Copter::ModeAuto::payload_place_run()
     if (!payload_place_run_should_run()) {
         zero_throttle_and_relax_ac();
         // set target to current position
-        wp_nav->init_loiter_target();
+        loiter_nav->init_target();
         return;
     }
 
@@ -1000,7 +1000,7 @@ void Copter::ModeAuto::payload_place_run_loiter()
     land_run_horizontal_control();
 
     // run loiter controller
-    wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+    loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
 
     // call attitude controller
     const float target_yaw_rate = 0;
@@ -2060,4 +2060,4 @@ float Copter::get_auto_yaw_rate_cds(void)
 
     // return zero turn rate (this should never happen)
     return 0.0f;
-}
+}

ArduCopter/mode_brake.cpp

@@ -9,9 +9,6 @@ bool Copter::ModeBrake::init(bool ignore_checks)
 {
     if (copter.position_ok() || ignore_checks) {
 
-        // set desired acceleration to zero
-        wp_nav->clear_pilot_desired_acceleration();
-
         // set target to current position
         wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);
 
@@ -47,7 +44,7 @@ void Copter::ModeBrake::run()
 
     // relax stop target if we might be landed
     if (ap.land_complete_maybe) {
-        wp_nav->loiter_soften_for_landing();
+        loiter_nav->soften_for_landing();
     }
 
     // if landed immediately disarm

ArduCopter/mode_land.cpp

@@ -14,8 +14,8 @@ bool Copter::ModeLand::init(bool ignore_checks)
     if (land_with_gps) {
         // set target to stopping point
         Vector3f stopping_point;
-        wp_nav->get_loiter_stopping_point_xy(stopping_point);
-        wp_nav->init_loiter_target(stopping_point);
+        loiter_nav->get_stopping_point_xy(stopping_point);
+        loiter_nav->init_target(stopping_point);
     }
 
     // initialize vertical speeds and leash lengths
@@ -57,7 +57,7 @@ void Copter::ModeLand::gps_run()
     // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
         zero_throttle_and_relax_ac();
-        wp_nav->init_loiter_target();
+        loiter_nav->init_target();
 
         // disarm when the landing detector says we've landed
         if (ap.land_complete) {

ArduCopter/mode_loiter.cpp

@@ -10,7 +10,7 @@ bool Copter::ModeLoiter::init(bool ignore_checks)
     if (copter.position_ok() || ignore_checks) {
 
         // set target to current position
-        wp_nav->init_loiter_target();
+        loiter_nav->init_target();
 
         // initialise position and desired velocity
         if (!pos_control->is_active_z()) {
@@ -34,7 +34,7 @@ bool Copter::ModeLoiter::do_precision_loiter()
         return false;        // don't move on the ground
     }
     // if the pilot *really* wants to move the vehicle, let them....
-    if (wp_nav->get_pilot_desired_acceleration().length() > 50.0f) {
+    if (loiter_nav->get_pilot_desired_acceleration().length() > 50.0f) {
         return false;
     }
     if (!copter.precland.target_acquired()) {
@@ -45,7 +45,7 @@ bool Copter::ModeLoiter::do_precision_loiter()
 
 void Copter::ModeLoiter::precision_loiter_xy()
 {
-    wp_nav->clear_pilot_desired_acceleration();
+    loiter_nav->clear_pilot_desired_acceleration();
     Vector2f target_pos, target_vel_rel;
     if (!copter.precland.get_target_position_cm(target_pos)) {
         target_pos.x = inertial_nav.get_position().x;
@@ -81,10 +81,10 @@ void Copter::ModeLoiter::run()
         update_simple_mode();
 
         // convert pilot input to lean angles
-        get_pilot_desired_lean_angles(target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
+        get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
 
         // process pilot's roll and pitch input
-        wp_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
+        loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
 
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
@@ -94,12 +94,12 @@ void Copter::ModeLoiter::run()
         target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
     } else {
         // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
-        wp_nav->clear_pilot_desired_acceleration();
+        loiter_nav->clear_pilot_desired_acceleration();
     }
 
     // relax loiter target if we might be landed
     if (ap.land_complete_maybe) {
-        wp_nav->loiter_soften_for_landing();
+        loiter_nav->soften_for_landing();
     }
 
     // Loiter State Machine Determination
@@ -123,13 +123,13 @@ void Copter::ModeLoiter::run()
         // force descent rate and call position controller
         pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
 #else
-        wp_nav->init_loiter_target();
+        loiter_nav->init_target();
         attitude_control->reset_rate_controller_I_terms();
         attitude_control->set_yaw_target_to_current_heading();
         pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
 #endif
-        wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
-        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
+        loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
         pos_control->update_z_controller();
         break;
 
@@ -153,10 +153,10 @@ void Copter::ModeLoiter::run()
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 
         // run loiter controller
-        wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+        loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
 
         // call attitude controller
-        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
 
         // update altitude target and call position controller
         pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
@@ -171,7 +171,7 @@ void Copter::ModeLoiter::run()
         } else {
             motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
         }
-        wp_nav->init_loiter_target();
+        loiter_nav->init_target();
         attitude_control->reset_rate_controller_I_terms();
         attitude_control->set_yaw_target_to_current_heading();
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0);
@@ -191,10 +191,10 @@ void Copter::ModeLoiter::run()
 #endif
 
         // run loiter controller
-        wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+        loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
 
         // call attitude controller
-        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
 
         // adjust climb rate using rangefinder
         if (copter.rangefinder_alt_ok()) {
@@ -214,10 +214,10 @@ void Copter::ModeLoiter::run()
 
 uint32_t Copter::ModeLoiter::wp_distance() const
 {
-    return wp_nav->get_loiter_distance_to_target();
+    return loiter_nav->get_distance_to_target();
 }
 
 int32_t Copter::ModeLoiter::wp_bearing() const
 {
-    return wp_nav->get_loiter_bearing_to_target();
+    return loiter_nav->get_bearing_to_target();
 }

ArduCopter/mode_poshold.cpp

@@ -100,7 +100,7 @@ bool Copter::ModePosHold::init(bool ignore_checks)
         poshold.pitch_mode = POSHOLD_LOITER;
         // 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();
     }else{
         // if not landed start in pilot override to avoid hard twitch
         poshold.roll_mode = POSHOLD_PILOT_OVERRIDE;
@@ -136,7 +136,7 @@ void Copter::ModePosHold::run()
 
     // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
-        wp_nav->init_loiter_target();
+        loiter_nav->init_target();
         zero_throttle_and_relax_ac();
         pos_control->relax_alt_hold_controllers(0.0f);
         return;
@@ -177,7 +177,7 @@ void Copter::ModePosHold::run()
 
     // relax loiter target if we might be landed
     if (ap.land_complete_maybe) {
-        wp_nav->loiter_soften_for_landing();
+        loiter_nav->soften_for_landing();
     }
 
     // if landed initialise loiter targets, set throttle to zero and exit
@@ -188,7 +188,7 @@ void Copter::ModePosHold::run()
         } else {
             motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
         }
-        wp_nav->init_loiter_target();
+        loiter_nav->init_target();
         attitude_control->reset_rate_controller_I_terms();
         attitude_control->set_yaw_target_to_current_heading();
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0);
@@ -409,7 +409,7 @@ void Copter::ModePosHold::run()
             poshold.pitch_mode = POSHOLD_BRAKE_TO_LOITER;
             poshold.brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER;
             // init loiter controller
-            wp_nav->init_loiter_target(inertial_nav.get_position());
+            loiter_nav->init_target(inertial_nav.get_position());
             // set delay to start of wind compensation estimate updates
             poshold.wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER;
         }
@@ -440,11 +440,11 @@ void Copter::ModePosHold::run()
                     poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw);
 
                     // run loiter controller
-                    wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+                    loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
 
                     // calculate final roll and pitch output by mixing loiter and brake controls
-                    poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, wp_nav->get_roll());
-                    poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, wp_nav->get_pitch());
+                    poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, loiter_nav->get_roll());
+                    poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, loiter_nav->get_pitch());
 
                     // check for pilot input
                     if (!is_zero(target_roll) || !is_zero(target_pitch)) {
@@ -471,11 +471,11 @@ void Copter::ModePosHold::run()
 
                 case POSHOLD_LOITER:
                     // run loiter controller
-                    wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+                    loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
 
                     // set roll angle based on loiter controller outputs
-                    poshold.roll = wp_nav->get_roll();
-                    poshold.pitch = wp_nav->get_pitch();
+                    poshold.roll = loiter_nav->get_roll();
+                    poshold.pitch = loiter_nav->get_pitch();
 
                     // update wind compensation estimate
                     poshold_update_wind_comp_estimate();

ArduCopter/mode_rtl.cpp

@@ -244,7 +244,7 @@ void Copter::ModeRTL::descent_start()
     _state_complete = false;
 
     // Set wp navigation target to above home
-    wp_nav->init_loiter_target(wp_nav->get_wp_destination());
+    loiter_nav->init_target(wp_nav->get_wp_destination());
 
     // initialise altitude target to stopping point
     pos_control->set_target_to_stopping_point_z();
@@ -265,7 +265,7 @@ void Copter::ModeRTL::descent_run()
     if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
         zero_throttle_and_relax_ac();
         // set target to current position
-        wp_nav->init_loiter_target();
+        loiter_nav->init_target();
         return;
     }
 
@@ -284,7 +284,7 @@ void Copter::ModeRTL::descent_run()
             update_simple_mode();
 
             // convert pilot input to lean angles
-            get_pilot_desired_lean_angles(target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
+            get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
 
             // record if pilot has overriden roll or pitch
             if (!is_zero(target_roll) || !is_zero(target_pitch)) {
@@ -300,17 +300,17 @@ void Copter::ModeRTL::descent_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // process roll, pitch inputs
-    wp_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
+    loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
 
     // run loiter controller
-    wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+    loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
 
     // call z-axis position controller
     pos_control->set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt);
     pos_control->update_z_controller();
 
     // roll & pitch from waypoint controller, yaw rate from pilot
-    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
+    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
 
     // check if we've reached within 20cm of final altitude
     _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20;
@@ -323,7 +323,7 @@ void Copter::ModeRTL::land_start()
     _state_complete = false;
 
     // Set wp navigation target to above home
-    wp_nav->init_loiter_target(wp_nav->get_wp_destination());
+    loiter_nav->init_target(wp_nav->get_wp_destination());
 
     // initialise position and desired velocity
     if (!pos_control->is_active_z()) {
@@ -356,7 +356,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
     if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
         zero_throttle_and_relax_ac();
         // set target to current position
-        wp_nav->init_loiter_target();
+        loiter_nav->init_target();
 
         // disarm when the landing detector says we've landed
         if (ap.land_complete && disarm_on_land) {

ArduCopter/mode_throw.cpp

@@ -79,7 +79,7 @@ void Copter::ModeThrow::run()
         stage = Throw_PosHold;
 
         // initialise the loiter target to the curent position and velocity
-        wp_nav->init_loiter_target();
+        loiter_nav->init_target();
 
         // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
         copter.set_auto_armed(true);
@@ -168,10 +168,10 @@ void Copter::ModeThrow::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);
 
         // call attitude controller
-        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f);
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), 0.0f);
 
         // call height controller
         pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);

ArduCopter/system.cpp

@@ -170,6 +170,7 @@ void Copter::init_ardupilot()
 #endif
 #if AC_AVOID_ENABLED == ENABLED
     wp_nav->set_avoidance(&avoid);
+    loiter_nav->set_avoidance(&avoid);
 #endif
 
     attitude_control->parameter_sanity_check();
@@ -602,6 +603,12 @@ void Copter::allocate_motors(void)
     }
     AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info);
 
+    loiter_nav = new AC_Loiter(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
+    if (loiter_nav == nullptr) {
+        AP_HAL::panic("Unable to allocate LoiterNav");
+    }
+    AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info);
+
 #if MODE_CIRCLE_ENABLED == ENABLED
     circle_nav = new AC_Circle(inertial_nav, *ahrs_view, *pos_control);
     if (circle_nav == nullptr) {