From 3382e0958059d442fde1bc999b7379a02b0493bf Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Wed, 30 Mar 2016 21:00:45 -0700
Subject: [PATCH] AP_NavEKF2: add height constraint during takeoff

---
 libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
index 38cf913ecc..d14ed37f5b 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
@@ -684,6 +684,11 @@ void NavEKF2_core::selectHeightForFusion()
         if (getTakeoffExpected() || getTouchdownExpected()) {
             posDownObsNoise *= frontend->gndEffectBaroScaler;
         }
+        // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
+        // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
+        if (motorsArmed && getTakeoffExpected()) {
+            hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
+        }
     } else {
         fuseHgtData = false;
     }