AP_NavEKF2: Add rangefinder body position offset to data buffer

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -77,6 +77,9 @@ void NavEKF2_core::readRangeFinder(void)
                 // limit the measured range to be no less than the on-ground range
                 rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd);
 
+                // get position in body frame for the current sensor
+                rangeDataNew.body_offset = frontend->_rng.get_pos_offset(sensorIndex);
+
                 // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
                 storedRange.push(rangeDataNew);
 

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -390,6 +390,7 @@ private:
     struct range_elements {
         float       rng;         // 0
         uint32_t    time_ms;     // 1
+        Vector3f    body_offset; // 2..4
     };
 
     struct tas_elements {