From ebd840165255d17c2db5d3ec8b695fd45184f092 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Thu, 8 Jul 2021 16:39:05 +0930
Subject: [PATCH] AC_AttitudeControl: AC_PosControl: support terrain following

---
 libraries/AC_AttitudeControl/AC_PosControl.cpp | 17 ++++++++++++++++-
 libraries/AC_AttitudeControl/AC_PosControl.h   |  5 ++++-
 2 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
index d01f84e61d..80effd1b01 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -334,7 +334,7 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
 ///     The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
 ///     The time constant also defines the time taken to achieve the maximum acceleration.
 ///     The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
-void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z)
+void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z, float pos_offset_z_buffer)
 {
     // remove terrain offsets for flat earth assumption
     _pos_target.z -= _pos_offset_z;
@@ -368,6 +368,9 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z)
         vel_max_z_cms = fabsf(vel_max_cms * dest_vector.z);
     }
 
+    // reduce speed if we are reaching the edge of our vertical buffer
+    vel_max_xy_cms *= pos_offset_z_scaler(pos_offset_z, pos_offset_z_buffer);
+
     Vector2f vel;
     Vector2f accel;
     shape_pos_vel_accel_xy(pos.xy(), vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(),
@@ -389,6 +392,18 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z)
     _accel_desired.z += _accel_offset_z;
 }
 
+
+/// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range
+float AC_PosControl::pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_buffer)
+{
+    if (is_zero(pos_offset_z_buffer)) {
+        return 1.0;
+    }
+    const Vector3f curr_pos = _inav.get_position();
+    float pos_offset_error_z = curr_pos.z - (_pos_target.z + pos_offset_z);
+    return constrain_float((1.0 - (fabsf(pos_offset_error_z) - 0.5 * pos_offset_z_buffer) / (0.5 * pos_offset_z_buffer)), 0.01, 1.0);
+}
+
 ///
 /// Lateral position controller
 ///
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h
index d0decc7ca3..baf4409fc2 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.h
+++ b/libraries/AC_AttitudeControl/AC_PosControl.h
@@ -57,7 +57,10 @@ public:
     /// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position.
     ///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
     ///     The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
-    void input_pos_xyz(const Vector3p& pos, float pos_offset_z);
+    void input_pos_xyz(const Vector3p& pos, float pos_offset_z, float pos_offset_z_buffer);
+
+    /// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range
+    float pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_buffer);
 
     ///
     /// Lateral position controller