From 950391df1264f30da115ac0be34526caa4d12a51 Mon Sep 17 00:00:00 2001
From: IamPete1 <33176108+IamPete1@users.noreply.github.com>
Date: Fri, 5 Apr 2019 19:28:09 +0100
Subject: [PATCH] SITL: add copter tailsitter

---
 libraries/SITL/SIM_Plane.h       |  1 +
 libraries/SITL/SIM_QuadPlane.cpp | 16 ++++++++++++++--
 2 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/libraries/SITL/SIM_Plane.h b/libraries/SITL/SIM_Plane.h
index b5db14f3ea..bfd0862e8e 100644
--- a/libraries/SITL/SIM_Plane.h
+++ b/libraries/SITL/SIM_Plane.h
@@ -99,6 +99,7 @@ protected:
     bool reverse_elevator_rudder;
     bool ice_engine;
     bool tailsitter;
+    bool copter_tailsitter;
     bool have_launcher;
     float launch_accel;
     float launch_time;
diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp
index 371d914e9b..37468a9b20 100644
--- a/libraries/SITL/SIM_QuadPlane.cpp
+++ b/libraries/SITL/SIM_QuadPlane.cpp
@@ -28,7 +28,9 @@ QuadPlane::QuadPlane(const char *frame_str) :
     // default to X frame
     const char *frame_type = "x";
     uint8_t motor_offset = 4;
-    
+
+    ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
+
     if (strstr(frame_str, "-octa-quad")) {
         frame_type = "octa-quad";
     } else if (strstr(frame_str, "-octaquad")) {
@@ -67,6 +69,11 @@ QuadPlane::QuadPlane(const char *frame_str) :
         frame_type = "tilttri";
         // fwd motor gives zero thrust
         thrust_scale = 0;
+    } else if (strstr(frame_str, "-copter_tailsitter")) {
+        frame_type = "+";
+        copter_tailsitter = true;
+        ground_behavior = GROUND_BEHAVIOR_TAILSITTER;
+        thrust_scale *= 1.5;
     }
     frame = Frame::find_frame(frame_type);
     if (frame == nullptr) {
@@ -94,7 +101,6 @@ QuadPlane::QuadPlane(const char *frame_str) :
     mass = frame->get_mass() * 1.5;
     frame->set_mass(mass);
 
-    ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
     lock_step_scheduled = true;
 }
 
@@ -116,6 +122,12 @@ void QuadPlane::update(const struct sitl_input &input)
 
     frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, &rpm[1], false);
 
+    // rotate frames for copter tailsitters
+    if (copter_tailsitter) {
+        quad_rot_accel.rotate(ROTATION_PITCH_270);
+        quad_accel_body.rotate(ROTATION_PITCH_270);
+    }
+
     // estimate voltage and current
     frame->current_and_voltage(battery_voltage, battery_current);