From 1fe6d77eebd8697363f05c34ce009478bd09aabe Mon Sep 17 00:00:00 2001
From: khanasif786 <asif.kgauri@gmail.com>
Date: Fri, 28 Jan 2022 21:01:33 +0530
Subject: [PATCH] Plane: Max altitude for FBWB using Fence altitude

---
 ArduPlane/Parameters.cpp |  2 +-
 ArduPlane/Plane.h        |  2 +-
 ArduPlane/altitude.cpp   | 59 ++++++++++++++++++++++++++++++----------
 ArduPlane/navigation.cpp |  2 +-
 4 files changed, 48 insertions(+), 17 deletions(-)

diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
index b307e6a005..30693d0fa8 100644
--- a/ArduPlane/Parameters.cpp
+++ b/ArduPlane/Parameters.cpp
@@ -662,7 +662,7 @@ const AP_Param::Info Plane::var_info[] = {
 
     // @Param: ALT_HOLD_FBWCM
     // @DisplayName: Minimum altitude for FBWB mode
-    // @Description: This is the minimum altitude in centimeters that FBWB and CRUISE modes will allow. If you attempt to descend below this altitude then the plane will level off. A value of zero means no limit.
+    // @Description: This is the minimum altitude in centimeters (above home) that FBWB and CRUISE modes will allow. If you attempt to descend below this altitude then the plane will level off. A value of zero means no limit.
     // @Units: cm
     // @User: Standard
     GSCALAR(FBWB_min_altitude_cm,   "ALT_HOLD_FBWCM", ALT_HOLD_FBW_CM),
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 239250f1bc..0810699add 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -833,7 +833,7 @@ private:
     void set_target_altitude_proportion(const Location &loc, float proportion);
     void constrain_target_altitude_location(const Location &loc1, const Location &loc2);
     int32_t calc_altitude_error_cm(void);
-    void check_fbwb_minimum_altitude(void);
+    void check_fbwb_altitude(void);
     void reset_offset_altitude(void);
     void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc);
     bool above_location_current(const Location &loc);
diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp
index ad7c3fafc7..6ad97848e1 100644
--- a/ArduPlane/altitude.cpp
+++ b/ArduPlane/altitude.cpp
@@ -361,26 +361,57 @@ int32_t Plane::calc_altitude_error_cm(void)
 }
 
 /*
-  check for FBWB_min_altitude_cm violation
+  check for FBWB_min_altitude_cm and fence min/max altitude
  */
-void Plane::check_fbwb_minimum_altitude(void)
+void Plane::check_fbwb_altitude(void)
 {
-    if (g.FBWB_min_altitude_cm == 0) {
-        return;
-    }
+    float max_alt_cm = 0.0;
+    float min_alt_cm = 0.0;
+    bool should_check_max = false;
+    bool should_check_min = false;
 
-#if AP_TERRAIN_AVAILABLE
-    if (target_altitude.terrain_following) {
-            // set our target terrain height to be at least the min set
-            if (target_altitude.terrain_alt_cm < g.FBWB_min_altitude_cm) {
-                target_altitude.terrain_alt_cm = g.FBWB_min_altitude_cm;
-            }
-            return;
+#if AP_FENCE_ENABLED
+    // taking fence max and min altitude (with margin)
+    const uint8_t enabled_fences = plane.fence.get_enabled_fences();
+    if ((enabled_fences & AC_FENCE_TYPE_ALT_MIN) != 0) {
+        min_alt_cm = plane.fence.get_safe_alt_min()*100.0;
+        should_check_min = true;
+    }
+    if ((enabled_fences & AC_FENCE_TYPE_ALT_MAX) != 0) {
+        max_alt_cm = plane.fence.get_safe_alt_max()*100.0;
+        should_check_max = true;
     }
 #endif
 
-    if (target_altitude.amsl_cm < home.alt + g.FBWB_min_altitude_cm) {
-        target_altitude.amsl_cm = home.alt + g.FBWB_min_altitude_cm;
+    if (g.FBWB_min_altitude_cm != 0) {
+        // FBWB min altitude exists
+        min_alt_cm = MAX(min_alt_cm, plane.g.FBWB_min_altitude_cm);
+        should_check_min = true;
+    }
+
+    if (!should_check_min && !should_check_max) {
+        return;
+    }
+
+//check if terrain following (min and max)
+#if AP_TERRAIN_AVAILABLE
+    if (target_altitude.terrain_following) {
+        // set our target terrain height to be at least the min set
+        if (should_check_max) {
+            target_altitude.terrain_alt_cm = MIN(target_altitude.terrain_alt_cm, max_alt_cm);
+        }
+        if (should_check_min) {
+            target_altitude.terrain_alt_cm = MAX(target_altitude.terrain_alt_cm, min_alt_cm);
+        }
+        return;
+    }
+#endif
+
+    if (should_check_max) {
+        target_altitude.amsl_cm = MIN(target_altitude.amsl_cm, home.alt + max_alt_cm);
+    }
+    if (should_check_min) {
+        target_altitude.amsl_cm = MAX(target_altitude.amsl_cm, home.alt + min_alt_cm);
     }
 }
 
diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp
index 0b1f7cf906..16c29f690d 100644
--- a/ArduPlane/navigation.cpp
+++ b/ArduPlane/navigation.cpp
@@ -411,7 +411,7 @@ void Plane::update_fbwb_speed_height(void)
         target_altitude.last_elevator_input = elevator_input;
     }
 
-    check_fbwb_minimum_altitude();
+    check_fbwb_altitude();
 
     altitude_error_cm = calc_altitude_error_cm();