From 16f021c46664a8554755275d7ffea4b24a5f26dd Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Wed, 12 Oct 2016 09:26:51 +1100
Subject: [PATCH] AP_NavEKF2: Correct range finder data for body frame position
 offset

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

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
index 7816683beb..b2e18e4515 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
@@ -650,6 +650,15 @@ void NavEKF2_core::selectHeightForFusion()
     readRangeFinder();
     rangeDataToFuse = storedRange.recall(rangeDataDelayed,imuDataDelayed.time_ms);
 
+    // correct range data for the body frame position offset relative to the IMU
+    // the corrected reading is the reading that would have been taken if the sensor was
+    // co-located with the IMU
+    if (rangeDataToFuse) {
+        Vector3f posOffsetBody = rangeDataDelayed.body_offset - accelPosOffset;
+        Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
+        rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
+    }
+
     // read baro height data from the sensor and check for new data in the buffer
     readBaroData();
     baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);