From 8bd1fc63d5968aca32303be434b953aa2948b451 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 10 Feb 2017 16:28:09 +1100
Subject: [PATCH] SITL: support pitch90 and pitch270 in FlightAxis

used for flying tailsitters
---
 libraries/SITL/SIM_FlightAxis.cpp | 35 ++++++++++++++++++++++++++++++-
 libraries/SITL/SIM_FlightAxis.h   |  4 +++-
 2 files changed, 37 insertions(+), 2 deletions(-)

diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp
index 091d66ef75..184a2f90ab 100644
--- a/libraries/SITL/SIM_FlightAxis.cpp
+++ b/libraries/SITL/SIM_FlightAxis.cpp
@@ -50,6 +50,28 @@ FlightAxis::FlightAxis(const char *home_str, const char *frame_str) :
     // FlightAxis sensor data is not good enough for EKF. Use fake EKF by default
     AP_Param::set_default_by_name("AHRS_EKF_TYPE", 10);
     AP_Param::set_default_by_name("INS_GYR_CAL", 0);
+
+    if (strstr(frame_str, "pitch270")) {
+        // rotate tailsitter airframes for fixed wing view
+        rotation = ROTATION_PITCH_270;
+    }
+    if (strstr(frame_str, "pitch90")) {
+        // rotate tailsitter airframes for fixed wing view
+        rotation = ROTATION_PITCH_90;
+    }
+
+    switch (rotation) {
+    case ROTATION_NONE:
+        break;
+    case ROTATION_PITCH_90:
+        att_rotation.from_euler(0, radians(90), 0);
+        break;
+    case ROTATION_PITCH_270:
+        att_rotation.from_euler(0, radians(270), 0);
+        break;
+    default:
+        AP_HAL::panic("Unsupported flightaxis rotation %u\n", (unsigned)rotation);
+    }
 }
 
 /*
@@ -284,9 +306,11 @@ void FlightAxis::update(const struct sitl_input &input)
                     state.m_orientationQuaternion_X,
                     -state.m_orientationQuaternion_Z);
     quat.rotation_matrix(dcm);
+
     gyro = Vector3f(radians(constrain_float(state.m_rollRate_DEGpSEC, -2000, 2000)),
-                    radians(constrain_float(state.m_pitchRate_DEGpSEC, -2000, 2000)),
+                   radians(constrain_float(state.m_pitchRate_DEGpSEC, -2000, 2000)),
                     -radians(constrain_float(state.m_yawRate_DEGpSEC, -2000, 2000))) * target_speedup;
+
     velocity_ef = Vector3f(state.m_velocityWorldU_MPS,
                              state.m_velocityWorldV_MPS,
                              state.m_velocityWorldW_MPS);
@@ -297,6 +321,15 @@ void FlightAxis::update(const struct sitl_input &input)
     accel_body(state.m_accelerationBodyAX_MPS2,
                state.m_accelerationBodyAY_MPS2,
                state.m_accelerationBodyAZ_MPS2);
+
+    if (rotation != ROTATION_NONE) {
+        dcm.transpose();
+        dcm = att_rotation * dcm;
+        dcm.transpose();
+        gyro.rotate(rotation);
+        accel_body.rotate(rotation);
+    }
+    
     // accel on the ground is nasty in realflight, and prevents helicopter disarm
     if (state.m_isTouchingGround) {
         Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds;
diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h
index aaf820771e..b153b1d1c7 100644
--- a/libraries/SITL/SIM_FlightAxis.h
+++ b/libraries/SITL/SIM_FlightAxis.h
@@ -166,7 +166,9 @@ private:
     double last_frame_count_s = 0;
     Vector3f position_offset;
     Vector3f last_velocity_ef;
-
+    Matrix3f att_rotation;
+    enum Rotation rotation = ROTATION_NONE;
+    
     const char *controller_ip = "127.0.0.1";
     uint16_t controller_port = 18083;
 };