From db1c7d9bdd0f75bfe66c155219ccc2132346a10a Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Mon, 13 Jan 2020 10:47:52 -0700
Subject: [PATCH] SITL: add support for AHRS_ORIENTATION=ROTATION_CUSTOM

---
 libraries/SITL/SIM_Aircraft.cpp   | 31 ++++++++++++++++++++++++-------
 libraries/SITL/SIM_Aircraft.h     |  4 +++-
 libraries/SITL/SIM_FlightAxis.cpp |  2 +-
 libraries/SITL/SITL.h             |  4 ++++
 4 files changed, 32 insertions(+), 9 deletions(-)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index 7ef208063d..b99361bfc5 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -75,9 +75,10 @@ Aircraft::Aircraft(const char *frame_str) :
     enum ap_var_type ptype;
     ahrs_orientation = (AP_Int8 *)AP_Param::find("AHRS_ORIENTATION", &ptype);
 
+    // ahrs_orientation->get() returns ROTATION_NONE here, regardless of the actual value
     enum Rotation imu_rotation = ahrs_orientation?(enum Rotation)ahrs_orientation->get():ROTATION_NONE;
-    ahrs_rotation_inv.from_rotation(imu_rotation);
-    ahrs_rotation_inv.transpose();
+    sitl->ahrs_rotation.from_rotation(imu_rotation);
+    sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed();
     last_imu_rotation = imu_rotation;
 
     terrain = reinterpret_cast<AP_Terrain *>(AP_Param::find_object("TERRAIN_"));
@@ -389,15 +390,31 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
 
     if (ahrs_orientation != nullptr) {
         enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get();
-
         if (imu_rotation != last_imu_rotation) {
-            ahrs_rotation_inv.from_rotation(imu_rotation);
-            ahrs_rotation_inv.transpose();
-            last_imu_rotation = imu_rotation;
+            // Surprisingly, Matrix3<T>::from_rotation(ROTATION_CUSTOM) is the identity matrix
+            // so we must deal with that here
+            if (imu_rotation == ROTATION_CUSTOM) {
+                if ((custom_roll == nullptr) || (custom_pitch == nullptr) || (custom_yaw == nullptr)) {
+                    enum ap_var_type ptype;
+                    custom_roll = (AP_Float *)AP_Param::find("AHRS_CUSTOM_ROLL", &ptype);
+                    custom_pitch = (AP_Float *)AP_Param::find("AHRS_CUSTOM_PIT", &ptype);
+                    custom_yaw = (AP_Float *)AP_Param::find("AHRS_CUSTOM_YAW", &ptype);
+                }
+                if ((custom_roll != nullptr) && (custom_pitch != nullptr) && (custom_yaw != nullptr)) {
+                    sitl->ahrs_rotation.from_euler(radians(*custom_roll), radians(*custom_pitch), radians(*custom_yaw));
+                    sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed();
+                } else {
+                    AP_HAL::panic("could not find one or more of parameters AHRS_CUSTOM_ROLL/PITCH/YAW");
+                }
+            } else {
+                sitl->ahrs_rotation.from_rotation(imu_rotation);
+                sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed();
+                last_imu_rotation = imu_rotation;
+            }
         }
         if (imu_rotation != ROTATION_NONE) {
             Matrix3f m = dcm;
-            m = m * ahrs_rotation_inv;
+            m = m * sitl->ahrs_rotation_inv;
 
             m.to_euler(&r, &p, &y);
             fdm.rollDeg  = degrees(r);
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index a4dd2b4e69..b9c911e01f 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -205,7 +205,9 @@ protected:
     // allow for AHRS_ORIENTATION
     AP_Int8 *ahrs_orientation;
     enum Rotation last_imu_rotation;
-    Matrix3f ahrs_rotation_inv;
+    AP_Float* custom_roll;
+    AP_Float* custom_pitch;
+    AP_Float* custom_yaw;
 
     enum GroundBehaviour {
         GROUND_BEHAVIOR_NONE = 0,
diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp
index c0ef0d061f..91f2bca42b 100644
--- a/libraries/SITL/SIM_FlightAxis.cpp
+++ b/libraries/SITL/SIM_FlightAxis.cpp
@@ -462,7 +462,7 @@ void FlightAxis::update(const struct sitl_input &input)
     Vector3f airspeed3d = dcm.mul_transpose(airspeed_3d_ef);
 
     if (last_imu_rotation != ROTATION_NONE) {
-        airspeed3d = airspeed3d * ahrs_rotation_inv;
+        airspeed3d = airspeed3d * sitl->ahrs_rotation_inv;
     }
     airspeed_pitot = MAX(airspeed3d.x,0);
 
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 8588a12549..932b23a9a4 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -124,6 +124,10 @@ public:
     static const struct AP_Param::GroupInfo var_info2[];
     static const struct AP_Param::GroupInfo var_info3[];
 
+    // Board Orientation (and inverse)
+    Matrix3f ahrs_rotation;
+    Matrix3f ahrs_rotation_inv;
+
     // noise levels for simulated sensors
     AP_Float baro_noise;  // in metres
     AP_Float baro_drift;  // in metres per second