From e62d6711c30dcf20d5436e25f234895891a53ba1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 30 Jan 2016 09:43:10 +1100
Subject: [PATCH] SITL: added very simple tiltrotor simulation

uses channel 9 to control tilt of rotors
---
 libraries/SITL/SIM_Multicopter.cpp | 22 +++++++++++++---------
 libraries/SITL/SIM_QuadPlane.cpp   | 22 ++++++++++++++++++++--
 libraries/SITL/SIM_QuadPlane.h     |  2 +-
 3 files changed, 34 insertions(+), 12 deletions(-)

diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp
index ca7b921ce0..b95eb3d89d 100644
--- a/libraries/SITL/SIM_Multicopter.cpp
+++ b/libraries/SITL/SIM_Multicopter.cpp
@@ -161,17 +161,21 @@ void Frame::calculate_forces(const Aircraft &aircraft,
         thrust += motor_speed[i] * thrust_scale; // newtons
     }
 
-    // rotational air resistance
-    const Vector3f &gyro = aircraft.get_gyro();
-    rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate;
-    rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate;
-    rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;
+    body_accel = Vector3f(0, 0, -thrust / mass);
 
-    // air resistance
-    Vector3f air_resistance = -aircraft.get_velocity_ef() * (GRAVITY_MSS/terminal_velocity);
+    if (terminal_rotation_rate > 0) {
+        // rotational air resistance
+        const Vector3f &gyro = aircraft.get_gyro();
+        rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate;
+        rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate;
+        rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;
+    }
 
-    body_accel = Vector3f(0, 0, -thrust / mass);
-    body_accel += aircraft.get_dcm().transposed() * air_resistance;    
+    if (terminal_velocity > 0) {
+        // air resistance
+        Vector3f air_resistance = -aircraft.get_velocity_ef() * (GRAVITY_MSS/terminal_velocity);
+        body_accel += aircraft.get_dcm().transposed() * air_resistance;
+    }
 
     // add some noise
     const float gyro_noise = radians(0.1);
diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp
index ec2d3a3eaf..165ddb4cf1 100644
--- a/libraries/SITL/SIM_QuadPlane.cpp
+++ b/libraries/SITL/SIM_QuadPlane.cpp
@@ -37,8 +37,13 @@ QuadPlane::QuadPlane(const char *home_str, const char *frame_str) :
     Plane(home_str, frame_str)
 {
     frame = &quad_frame;
-    // we use a very high terminal velocity to let the plane model handle the drag
-    frame->init(mass, 0.51, 100, 20*radians(360));
+    // we use zero terminal velocity to let the plane model handle the drag
+    frame->init(mass, 0.51, 0, 0);
+
+    // see if we want a tiltrotor
+    if (strstr(frame_str, "-tilt")) {
+        tiltrotors = true;
+    }
 }
 
 /*
@@ -56,6 +61,19 @@ 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 e88a2ac31d..e7615cef16 100644
--- a/libraries/SITL/SIM_QuadPlane.h
+++ b/libraries/SITL/SIM_QuadPlane.h
@@ -41,7 +41,7 @@ public:
     }
 private:
     Frame *frame;
-
+    bool tiltrotors;
 };
 
 } // namespace SITL