From 93a18e7dc8e4c8cc7cd26b675f91932544b039c6 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Sun, 19 Mar 2017 17:32:10 -0700
Subject: [PATCH] L1: Add loiter radius scaling based upon bank limits at sea
 level

---
 libraries/AP_L1_Control/AP_L1_Control.cpp | 38 ++++++++++++++++++++++-
 libraries/AP_L1_Control/AP_L1_Control.h   | 12 +++++--
 2 files changed, 47 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp
index e8c12d26bf..814f1c606b 100644
--- a/libraries/AP_L1_Control/AP_L1_Control.cpp
+++ b/libraries/AP_L1_Control/AP_L1_Control.cpp
@@ -30,6 +30,14 @@ 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: degrees
+    // @Range: 0 89
+    // @User: Advanced
+    AP_GROUPINFO_FRAME("LIM_BANK",   3, AP_L1_Control, _loiter_bank_limit, 0.0f, AP_PARAM_FRAME_PLANE),
+
     AP_GROUPEND
 };
 
@@ -127,6 +135,34 @@ 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
+{
+    // 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;
+    if(_spdHgtControl == nullptr) {
+        nominal_velocity_sea_level = 0.0f;
+    } else {
+        nominal_velocity_sea_level =  _spdHgtControl->get_target_airspeed();
+    }
+
+    float eas2tas_sq = sq(_ahrs.get_EAS2TAS());
+
+    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;
+        // select the requested radius, or the required altitude scale, whichever is safer
+        return MAX(sea_level_radius * eas2tas_sq, radius);
+    }
+}
+
 bool AP_L1_Control::reached_loiter_target(void)
 {
     return _WPcircle;
@@ -293,7 +329,7 @@ void AP_L1_Control::update_loiter(const struct Location &center_WP, float radius
 
     // scale loiter radius with square of EAS2TAS to allow us to stay
     // stable at high altitude
-    radius *= sq(_ahrs.get_EAS2TAS());
+    radius = loiter_radius(radius);
 
     // Calculate guidance gains used by PD loop (used during circle tracking)
     float omega = (6.2832f / _L1_period);
diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h
index ab8ce2f593..ad2f3e25a3 100644
--- a/libraries/AP_L1_Control/AP_L1_Control.h
+++ b/libraries/AP_L1_Control/AP_L1_Control.h
@@ -17,11 +17,13 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_Navigation/AP_Navigation.h>
+#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
 
 class AP_L1_Control : public AP_Navigation {
 public:
-    AP_L1_Control(AP_AHRS &ahrs)
-        : _ahrs(ahrs)
+    AP_L1_Control(AP_AHRS &ahrs, const AP_SpdHgtControl * spdHgtControl)
+        : _ahrs(ahrs),
+          _spdHgtControl(spdHgtControl)
     {
         AP_Param::setup_object_defaults(this, var_info);
     }
@@ -43,6 +45,7 @@ public:
     int32_t target_bearing_cd(void) const;
     float turn_distance(float wp_radius) const;
     float turn_distance(float wp_radius, float turn_angle) const;
+    float loiter_radius (const float loiter_radius) const;
     void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP);
     void update_loiter(const struct Location &center_WP, float radius, int8_t loiter_direction);
     void update_heading_hold(int32_t navigation_heading_cd);
@@ -72,6 +75,9 @@ private:
     // reference to the AHRS object
     AP_AHRS &_ahrs;
 
+    // pointer to the SpdHgtControl object
+    const AP_SpdHgtControl *_spdHgtControl;
+
     // lateral acceration in m/s required to fly to the
     // L1 reference point (+ve to right)
     float _latAccDem;
@@ -113,6 +119,8 @@ private:
     uint32_t _last_update_waypoint_us;
     bool _data_is_stale = true;
 
+    AP_Float _loiter_bank_limit;
+
     bool _reverse = false;
     float get_yaw();
     float get_yaw_sensor();