AC_Avoidance: add adjust_velocity_z

This allows reducing the climb rate to slow the vehicle's climb so it does not breach the altitude fence

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -67,6 +67,41 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel
     desired_vel.y = des_vel_xy.y;
 }
 
+// adjust vertical climb rate so vehicle does not break the vertical fence
+void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms)
+{
+    // exit immediately if disabled
+    if (_enabled == AC_AVOID_DISABLED) {
+        return;
+    }
+
+    // do not adjust climb_rate if level or descending
+    if (climb_rate_cms <= 0.0f) {
+        return;
+    }
+
+    // limit acceleration
+    float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);
+
+    // stop before breaching fence altitude
+    if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0) {
+
+        // calculate distance from vehicle to safe altitude
+        float veh_alt = get_alt_above_home();
+        float alt_diff_cm = _fence.get_safe_alt() * 100.0f - veh_alt;
+
+        // do not allow climbing if we've breached the safe altitude
+        if (alt_diff_cm <= 0.0f) {
+            climb_rate_cms = MIN(climb_rate_cms, 0.0f);
+            return;
+        }
+
+        // limit climb rate
+        const float max_speed = get_max_speed(kP, accel_cmss_limited, alt_diff_cm);
+        climb_rate_cms = MIN(max_speed, climb_rate_cms);
+    }
+}
+
 // adjust roll-pitch to push vehicle away from objects
 // roll and pitch value are in centi-degrees
 void AC_Avoid::adjust_roll_pitch(float &roll, float &pitch, float veh_angle_max)
@@ -297,6 +332,15 @@ Vector2f AC_Avoid::get_position() const
     return position_xy - diff;
 }
 
+/*
+ * Gets the altitude above home in cm
+ */
+float AC_Avoid::get_alt_above_home() const
+{
+    // vehicle's alt above ekf origin + ekf origin's alt above sea level - home's alt above sea level
+    return _inav.get_altitude() + _inav.get_origin().alt - _ahrs.get_home().alt;
+}
+
 /*
  * Computes the speed such that the stopping distance
  * of the vehicle will be exactly the input distance.

libraries/AC_Avoidance/AC_Avoid.h

@@ -39,6 +39,9 @@ public:
     void adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel);
     void adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel);
 
+    // adjust vertical climb rate so vehicle does not break the vertical fence
+    void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms);
+
     // adjust roll-pitch to push vehicle away from objects
     // roll and pitch value are in centi-degrees
     // angle_max is the user defined maximum lean angle for the vehicle in centi-degrees
@@ -83,9 +86,10 @@ private:
     void limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f& limit_direction, float limit_distance) const;
 
     /*
-     * Gets the current position, relative to home (not relative to EKF origin)
+     * Gets the current position or altitude, relative to home (not relative to EKF origin) in cm
      */
     Vector2f get_position() const;
+    float get_alt_above_home() const;
 
     /*
      * Computes the speed such that the stopping distance