From 8880635fe1d496db899f414f1dd77264938275f5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 22 Apr 2016 10:08:00 +1000
Subject: [PATCH] SITL: support vtail and elevon planes in builtin plane sim

remove old tiltrotor in favor of new tiltrotor code
---
 libraries/SITL/SIM_Frame.cpp     | 10 +++++-----
 libraries/SITL/SIM_Plane.cpp     | 20 ++++++++++++++++++++
 libraries/SITL/SIM_Plane.h       |  2 ++
 libraries/SITL/SIM_QuadPlane.cpp | 18 +-----------------
 libraries/SITL/SIM_QuadPlane.h   |  2 +-
 5 files changed, 29 insertions(+), 23 deletions(-)

diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp
index a1c7aa5d6c..17f2ff717d 100644
--- a/libraries/SITL/SIM_Frame.cpp
+++ b/libraries/SITL/SIM_Frame.cpp
@@ -102,15 +102,15 @@ static const Motor y6_motors[] =
 };
 
 /*
-  FireflyY6 is a Y6 with front motors tiltable
+  FireflyY6 is a Y6 with front motors tiltable using servo on channel 7
  */
 static const Motor firefly_motors[] =
 {
-    Motor(AP_MOTORS_MOT_1,  45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2, -1, 0, 0, AP_MOTORS_MOT_7, 0, 90),
-    Motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  5, -1, 0, 0, AP_MOTORS_MOT_7, 0, 90),
-    Motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6, -1, 0, 0, AP_MOTORS_MOT_7, 0, 90),
+    Motor(AP_MOTORS_MOT_1,  45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2, -1, 0, 0, AP_MOTORS_MOT_7, 0, -90),
+    Motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  5, -1, 0, 0, AP_MOTORS_MOT_7, 0, -90),
+    Motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6, -1, 0, 0, AP_MOTORS_MOT_7, 0, -90),
     Motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4),
-    Motor(AP_MOTORS_MOT_5,  45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1, -1, 0, 0, AP_MOTORS_MOT_7, 0, 90),
+    Motor(AP_MOTORS_MOT_5,  45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1, -1, 0, 0, AP_MOTORS_MOT_7, 0, -90),
     Motor(AP_MOTORS_MOT_6, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3)
 };
 
diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp
index f57647ac8c..514ae8f599 100644
--- a/libraries/SITL/SIM_Plane.cpp
+++ b/libraries/SITL/SIM_Plane.cpp
@@ -39,6 +39,11 @@ Plane::Plane(const char *home_str, const char *frame_str) :
     if (strstr(frame_str, "-revthrust")) {
         reverse_thrust = true;
     }
+    if (strstr(frame_str, "-elevon")) {
+        elevons = true;
+    } else if (strstr(frame_str, "-vtail")) {
+        vtail = true;
+    }
 }
 
 /*
@@ -195,6 +200,21 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel
     float elevator = (input.servos[1]-1500)/500.0f;
     float rudder   = (input.servos[3]-1500)/500.0f;
     float throttle;
+    if (elevons) {
+        // fake an elevon plane
+        float ch1 = aileron;
+        float ch2 = elevator;
+        aileron  = (ch2-ch1)/2.0f;
+        // the minus does away with the need for RC2_REV=-1
+        elevator = -(ch2+ch1)/2.0f;
+    } else if (vtail) {
+        // fake a vtail plane
+        float ch1 = elevator;
+        float ch2 = rudder;
+        // this matches VTAIL_OUTPUT==2
+        elevator = (ch2-ch1)/2.0f;
+        rudder   = (ch2+ch1)/2.0f;
+    }
 
     if (reverse_thrust) {
         throttle = constrain_float((input.servos[2]-1500)/500.0f, -1, 1);
diff --git a/libraries/SITL/SIM_Plane.h b/libraries/SITL/SIM_Plane.h
index 1cc03645c4..2b6d0577af 100644
--- a/libraries/SITL/SIM_Plane.h
+++ b/libraries/SITL/SIM_Plane.h
@@ -92,6 +92,8 @@ protected:
 
     float thrust_scale;
     bool reverse_thrust;
+    bool elevons;
+    bool vtail;
 
     float liftCoeff(float alpha) const;
     float dragCoeff(float alpha) const;
diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp
index a8f0b2028b..27373318c3 100644
--- a/libraries/SITL/SIM_QuadPlane.cpp
+++ b/libraries/SITL/SIM_QuadPlane.cpp
@@ -26,10 +26,6 @@ using namespace SITL;
 QuadPlane::QuadPlane(const char *home_str, const char *frame_str) :
     Plane(home_str, frame_str)
 {
-    // see if we want a tiltrotor
-    if (strstr(frame_str, "-tilt")) {
-        tiltrotors = true;
-    }
     // default to X frame
     const char *frame_type = "x";
 
@@ -51,6 +47,7 @@ QuadPlane::QuadPlane(const char *home_str, const char *frame_str) :
         frame_type = "y6";
     } else if (strstr(frame_str, "-firefly")) {
         frame_type = "firefly";
+        elevons = true;
     }
     frame = Frame::find_frame(frame_type);
     if (frame == nullptr) {
@@ -84,19 +81,6 @@ void QuadPlane::update(const struct sitl_input &input)
 
     frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body);
 
-    if (tiltrotors) {
-        // get rotor tilt from channel 9
-        float tilt = constrain_float(-90 * (input.servos[8]-1000)*0.001, -90, 0);
-
-        // rotate the rotational accel and body accel from multicopter
-        // model to take account of rotor tilt
-        Matrix3f rotation;
-        rotation.identity();
-        rotation.rotate(Vector3f(0, radians(tilt), 0));
-        quad_rot_accel = rotation * quad_rot_accel;
-        quad_accel_body = rotation * quad_accel_body;
-    }
-    
     rot_accel += quad_rot_accel;
     accel_body += quad_accel_body;
 
diff --git a/libraries/SITL/SIM_QuadPlane.h b/libraries/SITL/SIM_QuadPlane.h
index e7615cef16..0b14c49bfc 100644
--- a/libraries/SITL/SIM_QuadPlane.h
+++ b/libraries/SITL/SIM_QuadPlane.h
@@ -41,7 +41,7 @@ public:
     }
 private:
     Frame *frame;
-    bool tiltrotors;
+    bool elevons;
 };
 
 } // namespace SITL