From 747f3a8cfdc0c66f6f2579c9e7125f6eb7fbbf8a Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Mon, 17 Oct 2016 08:13:41 +1100
Subject: [PATCH] AP_HAL_SITL: Simulate range finder position offset

---
 libraries/AP_HAL_SITL/sitl_ins.cpp | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/libraries/AP_HAL_SITL/sitl_ins.cpp b/libraries/AP_HAL_SITL/sitl_ins.cpp
index 1d601baee9..17dfc8b917 100644
--- a/libraries/AP_HAL_SITL/sitl_ins.cpp
+++ b/libraries/AP_HAL_SITL/sitl_ins.cpp
@@ -86,6 +86,22 @@ uint16_t SITL_State::_ground_sonar(void)
 {
     float altitude = height_agl();
 
+    // sensor position offset in body frame
+    Vector3f relPosSensorBF = _sitl->rngfnd_pos_offset;
+
+    // adjust altitude for position of the sensor on the vehicle if position offset is non-zero
+    if (!relPosSensorBF.is_zero()) {
+        // get a rotation matrix following DCM conventions (body to earth)
+        Matrix3f rotmat;
+        rotmat.from_euler(radians(_sitl->state.rollDeg),
+                          radians(_sitl->state.pitchDeg),
+                          radians(_sitl->state.yawDeg));
+        // rotate the offset into earth frame
+        Vector3f relPosSensorEF = rotmat * relPosSensorBF;
+        // correct the altitude at the sensor
+        altitude -= relPosSensorEF.z;
+    }
+
     float voltage = 5.0f;
     if (fabsf(_sitl->state.rollDeg) < 90 &&
             fabsf(_sitl->state.pitchDeg) < 90) {