Plane: Fix loiter navigation accuracy

ArduPlane/Plane.h

@@ -38,6 +38,7 @@
 #include <AP_InertialSensor/AP_InertialSensor.h> // Inertial Sensor Library
 #include <AP_AccelCal/AP_AccelCal.h>                // interface and maths for accelerometer calibration
 #include <AP_AHRS/AP_AHRS.h>         // ArduPilot Mega DCM Library
+#include <AP_Baro/AP_Baro.h>
 #include <SRV_Channel/SRV_Channel.h>
 #include <AP_RangeFinder/AP_RangeFinder_config.h>     // Range finder library
 #include <Filter/Filter.h>                     // Filter library
@@ -242,7 +243,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, *AP_Baro::get_singleton(), aparm};
 
     // Attitude to servo controllers
     AP_RollController rollController{aparm};

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;

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;

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

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()
@@ -5314,7 +5311,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)
 

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
 

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

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

libraries/AP_Baro/AP_Baro_atmosphere.cpp

@@ -243,19 +243,22 @@ 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
+    return get_EAS2TAS_simple(alt_amsl, get_pressure_for_alt_amsl(alt_amsl));
+#endif
 }
 
-#endif // AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED || AP_SIM_ENABLED
-
 /*
   return geometric altitude difference in meters between current pressure and a
   given base_pressure in Pascal.
@@ -316,15 +319,21 @@ float AP_Baro::get_temperatureC_for_alt_amsl(const float alt_amsl)
     get_pressure_temperature_for_alt_amsl(alt_amsl, pressure, temp_K);
     return KELVIN_TO_C(temp_K);
 }
+#endif // AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED || AP_SIM_ENABLED
 
-// lookup expected pressure in Pa for a given altitude. Used for SITL backend
+// lookup expected pressure in Pa for a given altitude
 float AP_Baro::get_pressure_for_alt_amsl(const float alt_amsl)
 {
+#if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED || AP_SIM_ENABLED
     float pressure, temp_K;
     get_pressure_temperature_for_alt_amsl(alt_amsl, pressure, temp_K);
     return pressure;
+#else
+    const float sea_level_pressure = 101325.0f; // Standard sea level pressure in Pa
+    return sea_level_pressure *
+           powf(1.0f - (ISA_LAPSE_RATE * alt_amsl) / 288.15f, 5.255f);
+#endif
 }
-#endif // AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED
 
 /*
   return sea level pressure given a current altitude and pressure reading