From 35cf5e89a3e7f39003287bad167e9738fc48110b Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 21 Aug 2021 11:00:30 +0100
Subject: [PATCH 1/3] Update navigation_fixedwing.c

Add fixed wing Emergency Landing vertical descent rate control if altitude sensors working
---
 src/main/navigation/navigation_fixedwing.c | 22 ++++++++++++++++------
 1 file changed, 16 insertions(+), 6 deletions(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 484529944a..c738d22f29 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -243,18 +243,18 @@ static int8_t loiterDirection(void) {
 
     if (pidProfile()->loiter_direction == NAV_LOITER_YAW) {
 
-        if (rcCommand[YAW] < -250) { 
+        if (rcCommand[YAW] < -250) {
             loiterDirYaw = 1; //RIGHT //yaw is contrariwise
         }
 
-        if (rcCommand[YAW] > 250) { 
+        if (rcCommand[YAW] > 250) {
             loiterDirYaw = -1; //LEFT  //see annexCode in fc_core.c
         }
 
         dir = loiterDirYaw;
     }
 
-    if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) { 
+    if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) {
         dir *= -1;
     }
 
@@ -612,13 +612,23 @@ bool isFixedWingLandingDetected(void)
 /*-----------------------------------------------------------
  * FixedWing emergency landing
  *-----------------------------------------------------------*/
-void applyFixedWingEmergencyLandingController(void)
+void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
 {
-    // FIXME: Use altitude controller if available (similar to MC code)
     rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
-    rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
     rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
     rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
+
+    if (posControl.flags.estAltStatus >= EST_USABLE) {
+        updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
+        applyFixedWingAltitudeAndThrottleController(currentTimeUs);
+
+        if (isPitchAdjustmentValid) {
+            int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
+            rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
+        }
+    } else {
+        rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
+    }
 }
 
 /*-----------------------------------------------------------

From b485a6d1169b8d2a81065583e534c500f0e324dd Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 21 Aug 2021 13:42:22 +0100
Subject: [PATCH 2/3] Update navigation_fixedwing.c

---
 src/main/navigation/navigation_fixedwing.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index c738d22f29..a870e83ec9 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -651,7 +651,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
         applyFixedWingLaunchController(currentTimeUs);
     }
     else if (navStateFlags & NAV_CTL_EMERG) {
-        applyFixedWingEmergencyLandingController();
+        applyFixedWingEmergencyLandingController(currentTimeUs);
     }
     else {
 #ifdef NAV_FW_LIMIT_MIN_FLY_VELOCITY

From 5cbe2caad8448072313662d18f1e6755ec11aede Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 18 Sep 2021 10:40:10 +0100
Subject: [PATCH 3/3] remove redundant "if"

---
 src/main/navigation/navigation_fixedwing.c | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index a870e83ec9..9845bde147 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -622,10 +622,8 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
         updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
         applyFixedWingAltitudeAndThrottleController(currentTimeUs);
 
-        if (isPitchAdjustmentValid) {
-            int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
-            rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
-        }
+        int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
+        rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
     } else {
         rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
     }