From 6db93d8a21fc5cb39e7e862361d98092274216b2 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sat, 8 Oct 2016 09:11:10 +1100
Subject: [PATCH] AP_InertialSensor: Add parameters defining accelerometer
 position offset

---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 58 +++++++++++++++++++
 .../AP_InertialSensor/AP_InertialSensor.h     | 11 ++++
 2 files changed, 69 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index d098f58764..b12d562817 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -318,6 +318,64 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
     // @User: Advanced
     // @Values: 1:IMU 1,2:IMU 2,3:IMU 3
     AP_GROUPINFO("ACC_BODYFIX", 26, AP_InertialSensor, _acc_body_aligned, 2),
+
+    // @Param: POS1_X
+    // @DisplayName: IMU accelerometer X position
+    // @Description: X position of the first IMU Accelerometer in body frame.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: POS1_Y
+    // @DisplayName: IMU accelerometer Y position
+    // @Description: Y position of the first IMU accelerometer in body frame.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: POS1_Z
+    // @DisplayName: IMU accelerometer Z position
+    // @Description: Z position of the first IMU accelerometer in body frame.
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("POS1", 27, AP_InertialSensor, _accel_pos[0], 0.0f),
+
+    // @Param: POS2_X
+    // @DisplayName: IMU accelerometer X position
+    // @Description: X position of the second IMU accelerometer in body frame.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: POS2_Y
+    // @DisplayName: IMU accelerometer Y position
+    // @Description: Y position of the second IMU accelerometer in body frame.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: POS2_Z
+    // @DisplayName: IMU accelerometer Z position
+    // @Description: Z position of the second IMU accelerometer in body frame.
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("POS2", 28, AP_InertialSensor, _accel_pos[1], 0.0f),
+
+    // @Param: POS3_X
+    // @DisplayName: IMU accelerometer X position
+    // @Description: X position of the third IMU accelerometer in body frame.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: POS3_Y
+    // @DisplayName: IMU accelerometer Y position
+    // @Description: Y position of the third IMU accelerometer in body frame.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: POS3_Z
+    // @DisplayName: IMU accelerometer Z position
+    // @Description: Z position of the third IMU accelerometer in body frame.
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("POS3", 29, AP_InertialSensor, _accel_pos[2], 0.0f),
+
     /*
       NOTE: parameter indexes have gaps above. When adding new
       parameters check for conflicts carefully
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 9ce3b3c595..ec61848e9f 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -141,6 +141,14 @@ public:
     const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale[i]; }
     const Vector3f &get_accel_scale(void) const { return get_accel_scale(_primary_accel); }
 
+    // return a 3D vector defining the position offset of the IMU accelerometer in metres relative to the body frame origin
+    const Vector3f get_imu_pos_offset(uint8_t instance) const {
+        return _accel_pos[instance];
+    }
+    const Vector3f get_imu_pos_offset(void) const {
+        return _accel_pos[_primary_accel];
+    }
+
     // return the temperature if supported. Zero is returned if no
     // temperature is available
     float get_temperature(uint8_t instance) const { return _temperature[instance]; }
@@ -318,6 +326,9 @@ private:
     AP_Vector3f _accel_offset[INS_MAX_INSTANCES];
     AP_Vector3f _gyro_offset[INS_MAX_INSTANCES];
 
+    // accelerometer position offset in body frame
+    AP_Vector3f _accel_pos[INS_MAX_INSTANCES];
+
     // accelerometer max absolute offsets to be used for calibration
     float _accel_max_abs_offsets[INS_MAX_INSTANCES];