SITL: add copter-tailsitter

Tools/autotest/arduplane.py

@@ -534,7 +534,7 @@ def fly_mission(self, filename, mission_timeout=60.0, strict=True, quadplane=Fal
         self.wait_waypoint(1, num_wp, max_dist=60, timeout=mission_timeout)
         self.wait_groundspeed(0, 0.5, timeout=mission_timeout)
         if quadplane:
-            self.wait_statustext("Throttle disarmed", timeout=70)
+            self.wait_statustext("Throttle disarmed", timeout=200)
         else:
             self.wait_statustext("Auto disarmed", timeout=60)
         self.progress("Mission OK")
@@ -2993,6 +2993,10 @@ def fly_each_frame(self):
             quadplane = self.get_parameter('Q_ENABLE')
             if quadplane:
                 mission_file = "basic-quadplane.txt"
+            tailsitter = self.get_parameter('Q_TAILSIT_ENABLE')
+            if tailsitter:
+                # tailsitter needs extra re-boot to pick up the rotated AHRS view
+                self.reboot_sitl()
             self.wait_ready_to_arm()
             self.arm_vehicle()
             self.fly_mission(mission_file, strict=False, quadplane=quadplane, mission_timeout=400.0)

Tools/autotest/default_params/quadplane-copter_tailsitter.parm

@@ -0,0 +1,37 @@
+Q_ENABLE         1
+Q_FRAME_CLASS    1
+Q_FRAME_TYPE     0
+Q_TAILSIT_MOTMX  3
+Q_A_ACCEL_P_MAX  207069.531250
+Q_A_ACCEL_R_MAX  117885.953125
+Q_A_ACCEL_Y_MAX  12287.541992
+Q_A_ANGLE_BOOST  1.000000
+Q_A_ANG_LIM_TC   1.000000
+Q_A_ANG_PIT_P    18.000000
+Q_A_ANG_RLL_P    3.455029
+Q_A_ANG_YAW_P    2.068653
+Q_A_INPUT_TC     0.200000
+Q_A_RATE_FF_ENAB 1.000000
+Q_A_RATE_P_MAX   0.000000
+Q_A_RATE_R_MAX   0.000000
+Q_A_RATE_Y_MAX   0.000000
+Q_A_RAT_PIT_D    0.004973
+Q_A_RAT_PIT_FF   0.000000
+Q_A_RAT_PIT_FILT 10.000000
+Q_A_RAT_PIT_I    0.528246
+Q_A_RAT_PIT_IMAX 0.500000
+Q_A_RAT_PIT_P    0.528246
+Q_A_RAT_RLL_D    0.001000
+Q_A_RAT_RLL_FF   0.000000
+Q_A_RAT_RLL_FILT 10.000000
+Q_A_RAT_RLL_I    0.541613
+Q_A_RAT_RLL_IMAX 0.500000
+Q_A_RAT_RLL_P    0.541613
+Q_A_RAT_YAW_D    0.000000
+Q_A_RAT_YAW_FF   0.000000
+Q_A_RAT_YAW_FILT 1.073194
+Q_A_RAT_YAW_I    0.039192
+Q_A_RAT_YAW_IMAX 0.500000
+Q_A_RAT_YAW_P    0.391919
+Q_TAILSIT_THSCMX 1.5
+Q_TRANS_DECEL 8

Tools/autotest/pysim/vehicleinfo.py

@@ -262,6 +262,10 @@ def __init__(self):
                 "waf_target": "bin/arduplane",
                 "default_params_filename": ["default_params/plane.parm", "default_params/plane-jet.parm"],
             },
+            "quadplane-copter_tailsitter": {
+                "waf_target": "bin/arduplane",
+                "default_params_filename": ["default_params/quadplane.parm","default_params/quadplane-copter_tailsitter.parm"],
+            },
             "plane": {
                 "waf_target": "bin/arduplane",
                 "default_params_filename": "default_params/plane.parm",

libraries/SITL/SIM_Aircraft.cpp

@@ -690,15 +690,24 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
             break;
         }
         case GROUND_BEHAVIOR_TAILSITTER: {
-            // point straight up
+            // rotate normal refernce frame to get yaw angle, then rotate back
+            Matrix3f rot;
+            rot.from_rotation(ROTATION_PITCH_270);
             float r, p, y;
-            dcm.to_euler(&r, &p, &y);
+            (dcm * rot).to_euler(&r, &p, &y);
             y = y + yaw_rate * delta_time;
-            dcm.from_euler(0.0f, radians(90), y);
+            dcm.from_euler(0.0, 0.0, y);
+            rot.from_rotation(ROTATION_PITCH_90);
+            dcm *= rot;
             // X, Y movement tracks ground movement
             velocity_ef.x = gnd_movement.x;
             velocity_ef.y = gnd_movement.y;
+            if (velocity_ef.z > 0.0f) {
+                velocity_ef.z = 0.0f;
+            }
             gyro.zero();
+            gyro.x = yaw_rate;
+            use_smoothing = true;
             break;
         }
         }

libraries/SITL/SIM_Plane.h

@@ -99,6 +99,7 @@ class Plane : public Aircraft {
     bool reverse_elevator_rudder;
     bool ice_engine;
     bool tailsitter;
+    bool copter_tailsitter;
     bool have_launcher;
     float launch_accel;
     float launch_time;

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);