SITL: added very simple tiltrotor simulation

uses channel 9 to control tilt of rotors

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
+    body_accel = Vector3f(0, 0, -thrust / mass);
-    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;
 
-    // air resistance
+    if (terminal_rotation_rate > 0) {
-    Vector3f air_resistance = -aircraft.get_velocity_ef() * (GRAVITY_MSS/terminal_velocity);
+        // 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);
+    if (terminal_velocity > 0) {
-    body_accel += aircraft.get_dcm().transposed() * air_resistance;    
+        // 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);

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
+    // we use zero terminal velocity to let the plane model handle the drag
-    frame->init(mass, 0.51, 100, 20*radians(360));
+    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;
 

libraries/SITL/SIM_QuadPlane.h

@@ -41,7 +41,7 @@ public:
     }
 private:
     Frame *frame;
-
+    bool tiltrotors;
 };
 
 } // namespace SITL