diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 86783bed8c151..36c3871dc48ea 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -242,7 +242,7 @@ class Plane : public AP_Vehicle {
 #endif
 
     AP_TECS TECS_controller{ahrs, aparm, landing, MASK_LOG_TECS};
-    AP_L1_Control L1_controller{ahrs, &TECS_controller};
+    AP_L1_Control L1_controller{ahrs, TECS_controller, aparm};
 
     // Attitude to servo controllers
     AP_RollController rollController{aparm};
diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp
index 4727cc585015a..6613793ff75df 100644
--- a/ArduPlane/mode_autoland.cpp
+++ b/ArduPlane/mode_autoland.cpp
@@ -129,9 +129,10 @@ bool ModeAutoLand::_enter()
     // Try and minimize loiter radius by using the smaller of the waypoint loiter radius or 1/3 of the final WP distance
     const float loiter_radius = MIN(final_wp_dist * 0.333, fabsf(plane.aparm.loiter_radius));
 
-    // corrected_loiter_radius is the radius the vehicle will actually fly, this gets larger as altitude increases.
-    // Strictly this gets the loiter radius at the current altitude, really we want the loiter radius at final_wp_alt.
-    const float corrected_loiter_radius = plane.nav_controller->loiter_radius(loiter_radius);
+    // corrected_loiter_radius is the radius the vehicle will actually fly, this gets larger as altitude increases
+    const float corrected_loiter_radius =
+        plane.nav_controller->calc_corrected_loiter_radius(loiter_radius, NAN,
+                                                           final_wp_alt);
 
     cmd_loiter.id = MAV_CMD_NAV_LOITER_TO_ALT;
     cmd_loiter.p1 = loiter_radius;
diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp
index b37f3bdcb0311..4eebc7865bbbe 100644
--- a/ArduPlane/mode_loiter.cpp
+++ b/ArduPlane/mode_loiter.cpp
@@ -42,8 +42,9 @@ bool ModeLoiter::isHeadingLinedUp(const Location loiterCenterLoc, const Location
     // Return true if current heading is aligned to vector to targetLoc.
     // Tolerance is initially 10 degrees and grows at 10 degrees for each loiter circle completed.
 
-    // Corrected radius for altitude
-    const float loiter_radius = plane.nav_controller->loiter_radius(fabsf(plane.loiter.radius));
+    const float loiter_radius =
+        plane.nav_controller->calc_corrected_loiter_radius(
+            fabsf(plane.loiter.radius));
     if (!is_positive(loiter_radius)) {
         // Zero is invalid, protect against divide by zero for destination inside loiter radius case
         return true;
diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp
index b47160836aff8..2be42dc5f248b 100644
--- a/ArduPlane/navigation.cpp
+++ b/ArduPlane/navigation.cpp
@@ -321,7 +321,8 @@ void Plane::update_loiter_update_nav(uint16_t radius)
     if ((loiter.start_time_ms == 0 &&
          (control_mode == &mode_auto || control_mode == &mode_guided) &&
          auto_state.crosstrack &&
-         current_loc.get_distance(next_WP_loc) > 3 * nav_controller->loiter_radius(radius)) ||
+         current_loc.get_distance(next_WP_loc) >
+             3 * nav_controller->calc_corrected_loiter_radius(radius)) ||
         quadplane_qrtl_switch) {
         /*
           if never reached loiter point and using crosstrack and somewhat far away from loiter point
diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index a05ac4f33c907..06a7774f01690 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -1603,7 +1603,6 @@ def test_fence_breach_circle_at(self, loc, disable_on_breach=False):
         expected_radius = REALLY_BAD_FUDGE_FACTOR * want_radius
         self.set_parameters({
             "RTL_RADIUS": want_radius,
-            "NAVL1_LIM_BANK": 60,
             "FENCE_ACTION": 1, # AC_FENCE_ACTION_RTL_AND_LAND == 1. mavutil.mavlink.FENCE_ACTION_RTL == 4
         })
 
@@ -4026,7 +4025,6 @@ def FenceNoFenceReturnPoint(self):
             "FENCE_ACTION": 6,
             "FENCE_TYPE": 4,
             "RTL_RADIUS": want_radius,
-            "NAVL1_LIM_BANK": 60,
         })
         home_loc = self.mav.location()
         locs = [
@@ -4101,7 +4099,6 @@ def FenceNoFenceReturnPointInclusion(self):
             "FENCE_TYPE": 2,
             "FENCE_RADIUS": 300,
             "RTL_RADIUS": want_radius,
-            "NAVL1_LIM_BANK": 60,
         })
 
         self.clear_fence()
@@ -5381,7 +5378,6 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1):
         self.change_mode('AUTO')
         self.wait_ready_to_arm()
         self.arm_vehicle()
-        self.set_parameter("NAVL1_LIM_BANK", 50)
 
         self.wait_current_waypoint(2)
 
diff --git a/Tools/autotest/default_params/glider.parm b/Tools/autotest/default_params/glider.parm
index a64d9686edb83..cc471de14b93a 100644
--- a/Tools/autotest/default_params/glider.parm
+++ b/Tools/autotest/default_params/glider.parm
@@ -46,7 +46,6 @@ TECS_PTCH_FF_V0 25
 TECS_HDEM_TCONST 2
 
 NAVL1_PERIOD 20
-NAVL1_LIM_BANK 30
 
 SCHED_LOOP_RATE 200
 
diff --git a/Tools/autotest/default_params/plane-soaring.parm b/Tools/autotest/default_params/plane-soaring.parm
index a7f1aa65d47e3..cb3a552e00563 100644
--- a/Tools/autotest/default_params/plane-soaring.parm
+++ b/Tools/autotest/default_params/plane-soaring.parm
@@ -30,7 +30,6 @@ TECS_SPDWEIGHT   2.000000
 
 AIRSPEED_CRUISE 9.00
 
-NAVL1_LIM_BANK   60.000000
 NAVL1_PERIOD     10.000
 PTCH_RATE_FF 0.385000
 PTCH_RATE_P 0.040000
diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h
index 6fdbb02880e62..57cd09cadb038 100644
--- a/libraries/AP_Baro/AP_Baro.h
+++ b/libraries/AP_Baro/AP_Baro.h
@@ -111,7 +111,7 @@ class AP_Baro
     float get_temperature_from_altitude(float alt) const;
     float get_altitude_from_pressure(float pressure) const;
 
-    // EAS2TAS for SITL
+    // EAS2TAS at the given altitude AMSL
     static float get_EAS2TAS_for_alt_amsl(float alt_amsl);
 
 #if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED
diff --git a/libraries/AP_Baro/AP_Baro_atmosphere.cpp b/libraries/AP_Baro/AP_Baro_atmosphere.cpp
index ed26927cb9f89..53a0de12b21b3 100644
--- a/libraries/AP_Baro/AP_Baro_atmosphere.cpp
+++ b/libraries/AP_Baro/AP_Baro_atmosphere.cpp
@@ -243,18 +243,38 @@ float AP_Baro::get_EAS2TAS_extended(float altitude) const
     return sqrtf(SSL_AIR_DENSITY / density);
 }
 
+#endif // AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED || AP_SIM_ENABLED
+
 /*
   Given the geometric altitude (m)
   return scale factor that converts from equivalent to true airspeed
-  used by SITL only
 */
 float AP_Baro::get_EAS2TAS_for_alt_amsl(float alt_amsl)
 {
+#if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED
     const float density = get_air_density_for_alt_amsl(alt_amsl);
-    return sqrtf(SSL_AIR_DENSITY / MAX(0.00001,density));
-}
+#else
+    // estimate temperature at the given altitude using the standard lapse rate
+    float temp_at_alt_K = SSL_AIR_TEMPERATURE - ISA_LAPSE_RATE * alt_amsl;
 
-#endif // AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED || AP_SIM_ENABLED
+    // return if temperature is invalid
+    if (temp_at_alt_K <= 0.0f) {
+        return 1.0f;
+    }
+
+    // compute the pressure at the target altitude using the standard barometric
+    // formula for a gradient layer
+    float pressure_at_alt =
+        SSL_AIR_PRESSURE *
+        powf(temp_at_alt_K / SSL_AIR_TEMPERATURE,
+             GRAVITY_MSS / (ISA_GAS_CONSTANT * ISA_LAPSE_RATE));
+
+    // compute air density using ideal gas law
+    float density = pressure_at_alt / (ISA_GAS_CONSTANT * temp_at_alt_K);
+#endif
+
+    return sqrtf(SSL_AIR_DENSITY / MAX(0.00001, density));
+}
 
 /*
   return geometric altitude difference in meters between current pressure and a
diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp
index 4a5c83eb5d004..2da1a2597ef79 100644
--- a/libraries/AP_L1_Control/AP_L1_Control.cpp
+++ b/libraries/AP_L1_Control/AP_L1_Control.cpp
@@ -30,14 +30,6 @@ const AP_Param::GroupInfo AP_L1_Control::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("XTRACK_I",   2, AP_L1_Control, _L1_xtrack_i_gain, 0.02),
 
-    // @Param: LIM_BANK
-    // @DisplayName: Loiter Radius Bank Angle Limit
-    // @Description: The sealevel bank angle limit for a continous loiter. (Used to calculate airframe loading limits at higher altitudes). Setting to 0, will instead just scale the loiter radius directly
-    // @Units: deg
-    // @Range: 0 89
-    // @User: Advanced
-    AP_GROUPINFO("LIM_BANK",   3, AP_L1_Control, _loiter_bank_limit, 0.0f),
-
     AP_GROUPEND
 };
 
@@ -45,7 +37,7 @@ const AP_Param::GroupInfo AP_L1_Control::var_info[] = {
 // S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
 // Proceedings of the AIAA Guidance, Navigation and Control
 // Conference, Aug 2004. AIAA-2004-4900.
-// Modified to use PD control for circle tracking to enable loiter radius less than L1 length
+// Modified to use PID control for circle tracking to enable loiter radius less than L1 length
 // Modified to enable period and damping of guidance loop to be set explicitly
 // Modified to provide explicit control over capture angle
 
@@ -123,7 +115,6 @@ int32_t AP_L1_Control::target_bearing_cd(void) const
  */
 float AP_L1_Control::turn_distance(float wp_radius) const
 {
-    wp_radius *= sq(_ahrs.get_EAS2TAS());
     return MIN(wp_radius, _L1_dist);
 }
 
@@ -145,36 +136,41 @@ float AP_L1_Control::turn_distance(float wp_radius, float turn_angle) const
     return distance_90 * turn_angle / 90.0f;
 }
 
-float AP_L1_Control::loiter_radius(const float radius) const
+float AP_L1_Control::_calc_min_turn_radius(float indicated_airspeed,
+                                           float altitude_amsl) const
 {
-    // prevent an insane loiter bank limit
-    float sanitized_bank_limit = constrain_float(_loiter_bank_limit, 0.0f, 89.0f);
-    float lateral_accel_sea_level = tanf(radians(sanitized_bank_limit)) * GRAVITY_MSS;
 
-    float nominal_velocity_sea_level = 0.0f;
-    if(_tecs != nullptr) {
-        nominal_velocity_sea_level =  _tecs->get_target_airspeed();
-    }
+    float reference_ias = isnan(indicated_airspeed)
+                              ? _tecs.get_target_airspeed()
+                              : indicated_airspeed;
+    float reference_alt_amsl = isnan(altitude_amsl)
+                                   ? _baro.get_altitude_AMSL()
+                                   : altitude_amsl;
 
-    float eas2tas_sq = sq(_ahrs.get_EAS2TAS());
+    float tas_at_alt =
+        reference_ias * AP_Baro::get_EAS2TAS_for_alt_amsl(reference_alt_amsl);
 
-    if (is_zero(sanitized_bank_limit) || is_zero(nominal_velocity_sea_level) ||
-        is_zero(lateral_accel_sea_level)) {
-        // Missing a sane input for calculating the limit, or the user has
-        // requested a straight scaling with altitude. This will always vary
-        // with the current altitude, but will at least protect the airframe
-        return radius * eas2tas_sq;
-    } else {
-        float sea_level_radius = sq(nominal_velocity_sea_level) / lateral_accel_sea_level;
-        if (sea_level_radius > radius) {
-            // If we've told the plane that its sea level radius is unachievable fallback to
-            // straight altitude scaling
-            return radius * eas2tas_sq;
-        } else {
-            // select the requested radius, or the required altitude scale, whichever is safer
-            return MAX(sea_level_radius * eas2tas_sq, radius);
-        }
+    return sq(tas_at_alt) / (GRAVITY_MSS * tanf(_aparm.roll_limit));
+}
+
+bool AP_L1_Control::is_loiter_achievable(float radius, float indicated_airspeed,
+                                         float altitude_amsl) const
+{
+    if (radius < 0.0f) {
+        return false;
     }
+
+    float min_radius = _calc_min_turn_radius(indicated_airspeed, altitude_amsl);
+
+    return radius >= min_radius;
+}
+
+float AP_L1_Control::calc_corrected_loiter_radius(float original_radius,
+                                                  float indicated_airspeed,
+                                                  float altitude_amsl) const
+{
+    return MAX(original_radius,
+               _calc_min_turn_radius(indicated_airspeed, altitude_amsl));
 }
 
 bool AP_L1_Control::reached_loiter_target(void)
@@ -202,27 +198,55 @@ void AP_L1_Control::_prevent_indecision(float &Nu)
     }
 }
 
+void AP_L1_Control::_update_xtrack_integral(float error, float max_abs_error, float clamp)
+{
+    uint32_t now_us = AP_HAL::micros();
+    float dt = (now_us - _last_update_xtrack_i_us) * 1.0e-6f;
+    if (dt > 1) {
+        // Controller hasn't been called for an extended period of
+        // time. Reset it.
+        _L1_xtrack_i = 0.0f;
+    }
+    if (dt > 0.1) {
+        dt = 0.1;
+    }
+    _last_update_xtrack_i_us = now_us;
+
+    // Reset integral error component if gain parameter has changed. This allows
+    // for much easier tuning by having it re-converge each time it changes.
+    if (!is_positive(_L1_xtrack_i_gain) ||
+        !is_equal(_L1_xtrack_i_gain.get(), _L1_xtrack_i_gain_prev)) {
+        _L1_xtrack_i = 0;
+        _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain;
+
+        return;
+    }
+
+    // Return if error is too large
+    if (fabsf(error) >= max_abs_error) {
+        return;
+    }
+
+    // Compute integral error component to converge to a crosstrack of zero, and
+    // clamp it
+    _L1_xtrack_i += error * _L1_xtrack_i_gain * dt;
+    _L1_xtrack_i = constrain_float(_L1_xtrack_i, -clamp, clamp);
+}
+
 // update L1 control for waypoint navigation
 void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &next_WP, float dist_min)
 {
+    // Update nav. mode
+    if (_current_nav_mode != NavMode::WAYPOINT) {
+        _L1_xtrack_i = 0.0f; // Reset integral component on nav. mode change
+        _current_nav_mode = NavMode::WAYPOINT;
+    }
 
     Location _current_loc;
     float Nu;
     float xtrackVel;
     float ltrackVel;
 
-    uint32_t now = AP_HAL::micros();
-    float dt = (now - _last_update_waypoint_us) * 1.0e-6f;
-    if (dt > 1) {
-        // controller hasn't been called for an extended period of
-        // time.  Reinitialise it.
-        _L1_xtrack_i = 0.0f;
-    }
-    if (dt > 0.1) {
-        dt = 0.1;
-    }
-    _last_update_waypoint_us = now;
-
     // Calculate L1 gain required for specified damping
     float K_L1 = 4.0f * _L1_damping * _L1_damping;
 
@@ -310,18 +334,10 @@ void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &nex
         sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f);
         float Nu1 = asinf(sine_Nu1);
 
-        // compute integral error component to converge to a crosstrack of zero when traveling
-        // straight but reset it when disabled or if it changes. That allows for much easier
-        // tuning by having it re-converge each time it changes.
-        if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain.get(), _L1_xtrack_i_gain_prev)) {
-            _L1_xtrack_i = 0;
-            _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain;
-        } else if (fabsf(Nu1) < radians(5)) {
-            _L1_xtrack_i += Nu1 * _L1_xtrack_i_gain * dt;
-
-            // an AHRS_TRIM_X=0.1 will drift to about 0.08 so 0.1 is a good worst-case to clip at
-            _L1_xtrack_i = constrain_float(_L1_xtrack_i, -0.1f, 0.1f);
-        }
+        // Update the integral term up to 5ยบ of error
+        // An AHRS_TRIM_X=0.1 will drift to about 0.08 so 0.1 is a good
+        // worst-case to clip at
+        _update_xtrack_integral(Nu1, radians(5), 0.1f);
 
         // to converge to zero we must push Nu1 harder
         Nu1 += _L1_xtrack_i;
@@ -349,15 +365,18 @@ void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &nex
 // update L1 control for loitering
 void AP_L1_Control::update_loiter(const Location &center_WP, float radius, int8_t loiter_direction)
 {
+    // Update nav. mode
+    if (_current_nav_mode != NavMode::LOITER) {
+        _L1_xtrack_i = 0.0f; // Reset integral component on nav. mode change
+        _current_nav_mode = NavMode::LOITER;
+    }
+
     const float radius_unscaled = radius;
 
     Location _current_loc;
 
-    // scale loiter radius with square of EAS2TAS to allow us to stay
-    // stable at high altitude
-    radius = loiter_radius(fabsf(radius));
-
     // Calculate guidance gains used by PD loop (used during circle tracking)
+    // Calculate guidance gains used by PID loop (used during circle tracking)
     float omega = (6.2832f / _L1_period);
     float Kx = omega * omega;
     float Kv = 2.0f * _L1_damping * omega;
@@ -425,22 +444,27 @@ void AP_L1_Control::update_loiter(const Location &center_WP, float radius, int8_
     // keep crosstrack error for reporting
     _crosstrack_error = xtrackErrCirc;
 
-    // Calculate PD control correction to circle waypoint_ahrs.roll
-    float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv);
+    // Correct errors up to half the radius. Clamping to a value of 4
+    // produces good results, allowing for precise 50 m radius loiters at a
+    // speed of 22 m/s
+    _update_xtrack_integral(_crosstrack_error, radius / 2, 4.0f);
+
+    // Calculate PID control correction to circle waypoint_ahrs.roll
+    float latAccDemCircPID = xtrackErrCirc * Kx + xtrackVelCirc * Kv  + _L1_xtrack_i;
 
     // Calculate tangential velocity
     float velTangent = xtrackVelCap * float(loiter_direction);
 
-    // Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way
+    // Prevent PID demand from turning the wrong way by limiting the command when flying the wrong way
     if (ltrackVelCap < 0.0f && velTangent < 0.0f) {
-        latAccDemCircPD =  MAX(latAccDemCircPD, 0.0f);
+        latAccDemCircPID =  MAX(latAccDemCircPID, 0.0f);
     }
 
     // Calculate centripetal acceleration demand
     float latAccDemCircCtr = velTangent * velTangent / MAX((0.5f * radius), (radius + xtrackErrCirc));
 
-    // Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand
-    float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr);
+    // Sum PID control and centripetal acceleration to calculate lateral manoeuvre demand
+    float latAccDemCirc = loiter_direction * (latAccDemCircPID + latAccDemCircCtr);
 
     // Perform switchover between 'capture' and 'circle' modes at the
     // point where the commands cross over to achieve a seamless transfer
@@ -489,6 +513,11 @@ void AP_L1_Control::update_loiter(const Location &center_WP, float radius, int8_
 // update L1 control for heading hold navigation
 void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd)
 {
+    // Update nav. mode
+    if (_current_nav_mode != NavMode::HEADING_HOLD) {
+        _current_nav_mode = NavMode::HEADING_HOLD;
+    }
+
     // Calculate normalised frequency for tracking loop
     const float omegaA = 4.4428f/_L1_period; // sqrt(2)*pi/period
     // Calculate additional damping gain
@@ -531,6 +560,11 @@ void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd)
 // update L1 control for level flight on current heading
 void AP_L1_Control::update_level_flight(void)
 {
+    // Update nav. mode
+    if (_current_nav_mode != NavMode::LEVEL_FLIGHT) {
+        _current_nav_mode = NavMode::LEVEL_FLIGHT;
+    }
+
     // copy to _target_bearing_cd and _nav_bearing
     _target_bearing_cd = _ahrs.yaw_sensor;
     _nav_bearing = _ahrs.get_yaw();
diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h
index 20eb6e577713c..1b1fc49042286 100644
--- a/libraries/AP_L1_Control/AP_L1_Control.h
+++ b/libraries/AP_L1_Control/AP_L1_Control.h
@@ -17,13 +17,17 @@
 #include <AP_Param/AP_Param.h>
 #include <AP_Navigation/AP_Navigation.h>
 #include <AP_TECS/AP_TECS.h>
+#include <AP_Baro/AP_Baro.h>
+#include <AP_Vehicle/AP_FixedWing.h>
 #include <AP_Common/Location.h>
 
 class AP_L1_Control : public AP_Navigation {
 public:
-    AP_L1_Control(AP_AHRS &ahrs, const AP_TECS *tecs)
+    AP_L1_Control(const AP_AHRS &ahrs, const AP_TECS &tecs, const AP_FixedWing &aparm)
         : _ahrs(ahrs)
         , _tecs(tecs)
+        , _baro(*AP_Baro::get_singleton())
+        , _aparm(aparm)
     {
         AP_Param::setup_object_defaults(this, var_info);
     }
@@ -48,8 +52,12 @@ class AP_L1_Control : public AP_Navigation {
     int32_t target_bearing_cd(void) const override;
     float turn_distance(float wp_radius) const override;
     float turn_distance(float wp_radius, float turn_angle) const override;
-    float loiter_radius (const float loiter_radius) const override;
     void update_waypoint(const class Location &prev_WP, const class Location &next_WP, float dist_min = 0.0f) override;
+    bool is_loiter_achievable(float radius, float indicated_airspeed,
+                              float altitude_amsl) const override;
+    float calc_corrected_loiter_radius(float original_radius,
+                                       float indicated_airspeed,
+                                       float altitude_amsl) const override;
     void update_loiter(const class Location &center_WP, float radius, int8_t loiter_direction) override;
     void update_heading_hold(int32_t navigation_heading_cd) override;
     void update_level_flight(void) override;
@@ -76,10 +84,25 @@ class AP_L1_Control : public AP_Navigation {
 
 private:
     // reference to the AHRS object
-    AP_AHRS &_ahrs;
+    const AP_AHRS &_ahrs;
 
-    // pointer to the SpdHgtControl object
-    const AP_TECS *_tecs;
+    // reference to the TECS object
+    const AP_TECS &_tecs;
+
+    // reference to the barometer object
+    const AP_Baro &_baro;
+
+    // reference to the fixed wing parameters object
+    const AP_FixedWing &_aparm;
+
+    enum class NavMode {
+        NONE,
+        WAYPOINT,
+        LOITER,
+        HEADING_HOLD,
+        LEVEL_FLIGHT
+    };
+    NavMode _current_nav_mode = NavMode::NONE;
 
     // lateral acceration in m/s required to fly to the
     // L1 reference point (+ve to right)
@@ -116,13 +139,16 @@ class AP_L1_Control : public AP_Navigation {
 
     // integral feedback to correct crosstrack error. Used to ensure xtrack converges to zero.
     // For tuning purposes it's helpful to clear the integrator when it changes so a _prev is used
+    void _update_xtrack_integral(float error, float max_abs_error, float clamp);
+    uint32_t _last_update_xtrack_i_us;
     float _L1_xtrack_i = 0;
     AP_Float _L1_xtrack_i_gain;
     float _L1_xtrack_i_gain_prev = 0;
-    uint32_t _last_update_waypoint_us;
     bool _data_is_stale = true;
 
-    AP_Float _loiter_bank_limit;
+    // calculate minimum achievable turn radius based on indicated airspeed and
+    // altitude AMSL (assuming steady, coordinated, level turn)
+    float _calc_min_turn_radius(float ias = NAN, float altitude_amsl = NAN) const;
 
     // remember reached_loiter_target decision
     struct {
diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.cpp b/libraries/AP_Landing/AP_Landing_Deepstall.cpp
index 96b395a2e9ff8..55041d3bdac48 100644
--- a/libraries/AP_Landing/AP_Landing_Deepstall.cpp
+++ b/libraries/AP_Landing/AP_Landing_Deepstall.cpp
@@ -505,7 +505,8 @@ bool AP_Landing_Deepstall::terminate(void) {
 
 void AP_Landing_Deepstall::build_approach_path(bool use_current_heading)
 {
-    float loiter_radius = landing.nav_controller->loiter_radius(landing.aparm.loiter_radius);
+    float loiter_radius = landing.nav_controller->calc_corrected_loiter_radius(
+        landing.aparm.loiter_radius);
 
     Vector3f wind = landing.ahrs.wind_estimate();
     // TODO: Support a user defined approach heading
diff --git a/libraries/AP_Navigation/AP_Navigation.h b/libraries/AP_Navigation/AP_Navigation.h
index 26d25ad19a8c8..5fde0db211d19 100644
--- a/libraries/AP_Navigation/AP_Navigation.h
+++ b/libraries/AP_Navigation/AP_Navigation.h
@@ -56,9 +56,16 @@ class AP_Navigation {
     // mission when approaching a waypoint
     virtual float turn_distance(float wp_radius, float turn_angle) const = 0;
 
-    // return the target loiter radius for the current location that
-    // will not cause excessive airframe loading
-    virtual float loiter_radius(const float radius) const = 0;
+    // return true if the loiter radius is achievable at the current indicated
+    // airspeed and altitude AMSL, or at the given ones if specified
+    virtual bool is_loiter_achievable(float radius,
+        float indicated_airspeed = NAN, float altitude_amsl = NAN) const = 0;
+
+    // return the provided loiter radius if its achievable, and if not, the
+    // smallest radius that can be achieved at the current indicated airspeed
+    // and altitude AMSL, or at the given ones if specified
+    virtual float calc_corrected_loiter_radius(float original_radius,
+        float indicated_airspeed = NAN, float altitude_amsl = NAN) const = 0;
 
     // update the internal state of the navigation controller, given
     // the previous and next waypoints. This is the step function for
diff --git a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param
index 5bf3c82c55360..408c9d9dc7b75 100644
--- a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param
+++ b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param
@@ -334,7 +334,6 @@ MNT_RETRACT_Z,0
 MNT_TYPE,0
 NAV_CONTROLLER,1
 NAVL1_DAMPING,1
-NAVL1_LIM_BANK,60
 NAVL1_PERIOD,20
 NAVL1_XTRACK_I,0.02
 NTF_BUZZ_ENABLE,1
diff --git a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param
index 5eb9fc9cde642..44197a52087e4 100644
--- a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param
+++ b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param
@@ -337,7 +337,6 @@ MNT_RETRACT_Z,0
 MNT_TYPE,0
 NAV_CONTROLLER,1
 NAVL1_DAMPING,1
-NAVL1_LIM_BANK,60
 NAVL1_PERIOD,20
 NAVL1_XTRACK_I,0.02
 NTF_BUZZ_ENABLE,1