From 63b0cf4360b780d0fc9a31f0b6f8e4fc3a78c3a1 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 14 Mar 2016 15:51:15 +1100
Subject: [PATCH] EKF: Fix baro height offset bug

---
 EKF/ekf.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index 51f3a64fa9..252cb54e98 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -398,8 +398,12 @@ bool Ekf::initialiseFilter(void)
 
 		// if we are not using the baro height as the primary source, then calculate an offset relative to the origin
 		// so it can be used as a backup
-		baroSample baro_newest = _baro_buffer.get_newest();
-		_baro_hgt_offset = baro_newest.hgt;
+		if (_params.vdist_sensor_type != VDIST_SENSOR_BARO) {
+			baroSample baro_newest = _baro_buffer.get_newest();
+			_baro_hgt_offset = baro_newest.hgt - _hgt_at_alignment;
+		} else {
+			_baro_hgt_offset = 0.0f;
+		}
 
 		// set the velocity to the GPS measurement (by definition, the initial position and height is at the origin)
 		resetVelocity();