From c5adc0b549eb784dc1f8a68aabb91f25e009a28b Mon Sep 17 00:00:00 2001
From: Nicola De Pasquale <ndp7c7c0@gmail.com>
Date: Sun, 19 May 2019 02:36:21 +0200
Subject: [PATCH] first commit

fixes
---
 src/main/cli/settings.c      | 1 +
 src/main/flight/gps_rescue.c | 7 ++++++-
 src/main/flight/gps_rescue.h | 1 +
 3 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index e74e314b4c..028efda08a 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -908,6 +908,7 @@ const clivalue_t valueTable[] = {
     { "gps_rescue_min_sats",        VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) },
     { "gps_rescue_min_dth",         VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 50, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) },
     { "gps_rescue_allow_arming_without_fix", VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowArmingWithoutFix) },
+    { "gps_rescue_climb_to_max_reached_alt", VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowClimbToMaxReachedAlt) },
 #ifdef USE_MAG
     { "gps_rescue_use_mag",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) },
 #endif
diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c
index 4c15e30480..00061679ad 100644
--- a/src/main/flight/gps_rescue.c
+++ b/src/main/flight/gps_rescue.c
@@ -150,6 +150,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
     .useMag = GPS_RESCUE_USE_MAG,
     .targetLandingAltitudeM = 5,
     .targetLandingDistanceM = 10,
+    .allowClimbToMaxReachedAlt = true,
 );
 
 static uint16_t rescueThrottle;
@@ -549,7 +550,11 @@ void updateGPSRescueState(void)
         }
 
         rescueState.intent.targetGroundspeed = 500;
-        rescueState.intent.targetAltitudeCm = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500);
+        if (gpsRescueConfig()->allowClimbToMaxReachedAlt) {
+            rescueState.intent.targetAltitudeCm = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500);
+        } else {
+            rescueState.intent.targetAltitudeCm = gpsRescueConfig()->initialAltitudeM * 100;
+        }
         rescueState.intent.crosstrack = true;
         rescueState.intent.minAngleDeg = 10;
         rescueState.intent.maxAngleDeg = 15;
diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h
index bb03284885..bb80d4be7e 100644
--- a/src/main/flight/gps_rescue.h
+++ b/src/main/flight/gps_rescue.h
@@ -39,6 +39,7 @@ typedef struct gpsRescue_s {
     uint8_t useMag;
     uint16_t targetLandingAltitudeM; //meters
     uint16_t targetLandingDistanceM; //meters
+    uint8_t allowClimbToMaxReachedAlt;
 } gpsRescueConfig_t;
 
 PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);