From c66241d9acc33ca46fbaf61170bbe6429a1036a3 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 3 Mar 2019 11:15:45 +0100
Subject: [PATCH] Set max allowed altitude for both FW and MR

---
 src/main/fc/settings.yaml        |  3 +++
 src/main/navigation/navigation.c | 15 ++++++++++++++-
 src/main/navigation/navigation.h |  1 +
 3 files changed, 18 insertions(+), 1 deletion(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index bb718961cd..b98e369c35 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1307,6 +1307,9 @@ groups:
       - name: nav_max_terrain_follow_alt
         field: general.max_terrain_follow_altitude
         max: 1000
+      - name: nav_max_altitude
+        field: general.max_altitude
+        max: 65000
       - name: nav_rth_altitude
         field: general.rth_altitude
         max: 65000
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 5575dc8348..da3a484af5 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -77,7 +77,7 @@ PG_DECLARE_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList);
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 3);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {
@@ -112,6 +112,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .rth_home_altitude = 0,
         .rth_abort_threshold = 50000, // 500m - should be safe for all aircraft
         .max_terrain_follow_altitude = 100,     // max 1m altitude in terrain following mode
+        .max_altitude = 0,                      // Altitude limit disabled by default
     },
 
     // MC-specific
@@ -2289,6 +2290,18 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
         posControl.desiredState.pos.z = altitudeToUse;
     }
     else {
+
+        /* 
+         * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
+         * In other words, when altitude is reached, allow it only to shrink
+         */
+        if (navConfig()->general.max_altitude > 0 && 
+            altitudeToUse >= navConfig()->general.max_altitude &&
+            desiredClimbRate > 0
+        ) {
+            desiredClimbRate = 0;
+        }
+
         if (STATE(FIXED_WING)) {
             // Fixed wing climb rate controller is open-loop. We simply move the known altitude target
             float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 6a7c9d19e9..989c154c88 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -153,6 +153,7 @@ typedef struct navConfig_s {
         uint16_t min_rth_distance;              // 0 Disables. Minimal distance for RTH in cm, otherwise it will just autoland
         uint16_t rth_abort_threshold;           // Initiate emergency landing if during RTH we get this much [cm] away from home
         uint16_t max_terrain_follow_altitude;   // Max altitude to be used in SURFACE TRACKING mode
+        uint16_t max_altitude;                  // Max altitude when in AltHold mode (not Surface Following)
     } general;
 
     struct {