From 74be9f0bda3aa899a000db042516806a1f88b8e5 Mon Sep 17 00:00:00 2001
From: Jacob Walser <jwalser90@gmail.com>
Date: Thu, 8 Mar 2018 21:26:56 -0500
Subject: [PATCH] AP_AHRS: Add support for custom board orientations

---
 libraries/AP_AHRS/AP_AHRS.cpp | 47 ++++++++++++++++++++++++++++++++++-
 libraries/AP_AHRS/AP_AHRS.h   | 12 ++++-----
 2 files changed, 52 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index 134f57f8e3..9e12aea45c 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -94,7 +94,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
     // @Param: ORIENTATION
     // @DisplayName: Board Orientation
     // @Description: Overall board orientation relative to the standard orientation for the board type. This rotates the IMU and compass readings to allow the board to be oriented in your vehicle at any 90 or 45 degree angle. This option takes affect on next boot. After changing you will need to re-level your vehicle.
-    // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315
+    // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315,100:Custom
     // @User: Advanced
     AP_GROUPINFO("ORIENTATION", 9, AP_AHRS, _board_orientation, 0),
 
@@ -128,6 +128,33 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
     AP_GROUPINFO("EKF_TYPE",  14, AP_AHRS, _ekf_type, 2),
 #endif
 
+    // @Param: CUSTOM_ROLL
+    // @DisplayName: Board orientation roll offset
+    // @Description: Autopilot mounting position roll offset. Positive values = roll right, negative values = roll left. This parameter is only used when AHRS_ORIENTATION is set to CUSTOM.
+    // @Range: -180 180
+    // @Units: deg
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("CUSTOM_ROLL", 15, AP_AHRS, _custom_roll, 0),
+
+    // @Param: CUSTOM_PIT
+    // @DisplayName: Board orientation pitch offset
+    // @Description: Autopilot mounting position pitch offset. Positive values = pitch up, negative values = pitch down. This parameter is only used when AHRS_ORIENTATION is set to CUSTOM.
+    // @Range: -180 180
+    // @Units: deg
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("CUSTOM_PIT", 16, AP_AHRS, _custom_pitch, 0),
+
+    // @Param: CUSTOM_YAW
+    // @DisplayName: Board orientation yaw offset
+    // @Description: Autopilot mounting position yaw offset. Positive values = yaw right, negative values = yaw left. This parameter is only used when AHRS_ORIENTATION is set to CUSTOM.
+    // @Range: -180 180
+    // @Units: deg
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("CUSTOM_YAW", 17, AP_AHRS, _custom_yaw, 0),
+
     AP_GROUPEND
 };
 
@@ -185,6 +212,24 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_
     }
 }
 
+// Set the board mounting orientation, may be called while disarmed
+void AP_AHRS::set_orientation()
+{
+    enum Rotation orientation = (enum Rotation)_board_orientation.get();
+    if (orientation != ROTATION_CUSTOM) {
+        AP::ins().set_board_orientation(orientation);
+        if (_compass != nullptr) {
+            _compass->set_board_orientation(orientation);
+        }
+    } else {
+        _custom_rotation.from_euler(_custom_roll, _custom_pitch, _custom_yaw);
+        AP::ins().set_board_orientation(orientation, &_custom_rotation);
+        if (_compass != nullptr) {
+            _compass->set_board_orientation(orientation, &_custom_rotation);
+        }
+    }
+}
+
 // return a ground speed estimate in m/s
 Vector2f AP_AHRS::groundspeed_vector(void)
 {
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 4216c1cb2b..7bf12dd7b4 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -161,12 +161,7 @@ public:
 
     // allow for runtime change of orientation
     // this makes initial config easier
-    void set_orientation() {
-        AP::ins().set_board_orientation((enum Rotation)_board_orientation.get());
-        if (_compass != nullptr) {
-            _compass->set_board_orientation((enum Rotation)_board_orientation.get());
-        }
-    }
+    void set_orientation();
 
     void set_airspeed(AP_Airspeed *airspeed) {
         _airspeed = airspeed;
@@ -583,6 +578,11 @@ protected:
     AP_Int8 _gps_minsats;
     AP_Int8 _gps_delay;
     AP_Int8 _ekf_type;
+    AP_Float _custom_roll;
+    AP_Float _custom_pitch;
+    AP_Float _custom_yaw;
+
+    Matrix3f _custom_rotation;
 
     // flags structure
     struct ahrs_flags {