From 24f9e7365defa6ba70dc18886bf5c65f8273bdb2 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sat, 8 Oct 2016 09:29:42 +1100
Subject: [PATCH] AP_RangeFinder: Add parameters defining sensor position
 offset

---
 libraries/AP_RangeFinder/RangeFinder.cpp | 78 +++++++++++++++++++++++-
 libraries/AP_RangeFinder/RangeFinder.h   |  9 +++
 2 files changed, 86 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index 8ea66036fd..a60bc3724d 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/RangeFinder.cpp
@@ -130,7 +130,26 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_ADDR", 23, RangeFinder, _address[0], 0),
 
-#if RANGEFINDER_MAX_INSTANCES > 1
+    // @Param: 1_POS_X
+    // @DisplayName:  X position offset
+    // @Description: X position of the first rangefinder in body frame. Use the zero range datum point if supplied.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: 1_POS_Y
+    // @DisplayName: Y position offset
+    // @Description: Y position of the first rangefinder in body frame. Use the zero range datum point if supplied.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: 1_POS_Z
+    // @DisplayName: Z position offset
+    // @Description: Z position of the first rangefinder in body frame. Use the zero range datum point if supplied.
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("1_POS", 49, RangeFinder, _pos_offset[0], 0.0f),
+
+    #if RANGEFINDER_MAX_INSTANCES > 1
     // @Param: 2_TYPE
     // @DisplayName: Second Rangefinder type
     // @Description: What type of rangefinder device that is connected
@@ -223,6 +242,25 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("2_ADDR", 24, RangeFinder, _address[1], 0),
 
+    // @Param: 2_POS_X
+    // @DisplayName:  X position offset
+    // @Description: X position of the second rangefinder in body frame. Use the zero range datum point if supplied.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: 2_POS_Y
+    // @DisplayName: Y position offset
+    // @Description: Y position of the second rangefinder in body frame. Use the zero range datum point if supplied.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: 2_POS_Z
+    // @DisplayName: Z position offset
+    // @Description: Z position of the second rangefinder in body frame. Use the zero range datum point if supplied.
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("2_POS", 50, RangeFinder, _pos_offset[1], 0.0f),
+
 #endif
 
 #if RANGEFINDER_MAX_INSTANCES > 2
@@ -309,6 +347,25 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("3_ADDR", 36, RangeFinder, _address[2], 0),
 
+    // @Param: 3_POS_X
+    // @DisplayName:  X position offset
+    // @Description: X position of the third rangefinder in body frame. Use the zero range datum point if supplied.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: 3_POS_Y
+    // @DisplayName: Y position offset
+    // @Description: Y position of the third rangefinder in body frame. Use the zero range datum point if supplied.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: 3_POS_Z
+    // @DisplayName: Z position offset
+    // @Description: Z position of the third rangefinder in body frame. Use the zero range datum point if supplied.
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("3_POS", 51, RangeFinder, _pos_offset[2], 0.0f),
+
 #endif
 
 #if RANGEFINDER_MAX_INSTANCES > 3
@@ -394,6 +451,25 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @Increment: 1
     // @User: Advanced
     AP_GROUPINFO("4_ADDR", 48, RangeFinder, _address[3], 0),
+
+    // @Param: 4_POS_X
+    // @DisplayName:  X position offset
+    // @Description: X position of the fourth rangefinder in body frame. Use the zero range datum point if supplied.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: 4_POS_Y
+    // @DisplayName: Y position offset
+    // @Description: Y position of the fourth rangefinder in body frame. Use the zero range datum point if supplied.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: 4_POS_Z
+    // @DisplayName: Z position offset
+    // @Description: Z position of the fourth rangefinder in body frame. Use the zero range datum point if supplied.
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("4_POS", 52, RangeFinder, _pos_offset[3], 0.0f),
 #endif
     
     AP_GROUPEND
diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h
index 9f46e9d7e8..c9386f1388 100644
--- a/libraries/AP_RangeFinder/RangeFinder.h
+++ b/libraries/AP_RangeFinder/RangeFinder.h
@@ -92,6 +92,7 @@ public:
     AP_Int8  _ground_clearance_cm[RANGEFINDER_MAX_INSTANCES];
     AP_Int8  _address[RANGEFINDER_MAX_INSTANCES];
     AP_Int16 _powersave_range;
+    AP_Vector3f _pos_offset[RANGEFINDER_MAX_INSTANCES]; // position offset in body frame
 
     static const struct AP_Param::GroupInfo var_info[];
     
@@ -181,6 +182,14 @@ public:
      */
     bool pre_arm_check() const;
 
+    // return a 3D vector defining the position offset of the sensor in metres relative to the body frame origin
+    const Vector3f get_pos_offset(uint8_t instance) const {
+        return _pos_offset[instance];
+    }
+    const Vector3f get_pos_offset(void) const {
+        return _pos_offset[primary_instance];
+    }
+
 private:
     RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];
     AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES];