SITL: Add HELI_DUAL_FRAME and HELI_COMPOUND_FRAME.

Tools/autotest/sim_vehicle.sh

@@ -252,6 +252,16 @@ case $FRAME in
 	BUILD_TARGET="sitl-heli"
         MODEL="heli"
 	;;
+    heli-dual)
+  BUILD_TARGET="sitl-heli-dual"
+        EXTRA_SIM="$EXTRA_SIM --frame=heli-dual"
+        MODEL="heli-dual"
+  ;;
+    heli-compound)
+  BUILD_TARGET="sitl-heli-compound"
+        EXTRA_SIM="$EXTRA_SIM --frame=heli-compound"
+        MODEL="heli-compound"
+  ;;
     IrisRos)
 	BUILD_TARGET="sitl"
 	;;

libraries/AP_HAL_SITL/SITL_cmdline.cpp

@@ -53,20 +53,22 @@ static const struct {
     const char *name;
     Aircraft *(*constructor)(const char *home_str, const char *frame_str);
 } model_constructors[] = {
-    { "+",         MultiCopter::create },
+    { "+",                  MultiCopter::create },
-    { "quad",      MultiCopter::create },
+    { "quad",               MultiCopter::create },
-    { "copter",    MultiCopter::create },
+    { "copter",             MultiCopter::create },
-    { "x",         MultiCopter::create },
+    { "x",                  MultiCopter::create },
-    { "hexa",      MultiCopter::create },
+    { "hexa",               MultiCopter::create },
-    { "octa",      MultiCopter::create },
+    { "octa",               MultiCopter::create },
-    { "heli",      Helicopter::create },
+    { "heli",               Helicopter::create },
-    { "rover",     Rover::create },
+    { "heli-dual",          Helicopter::create },
-    { "crrcsim",   CRRCSim::create },
+    { "heli-compound",      Helicopter::create },
-    { "jsbsim",    JSBSim::create },
+    { "rover",              Rover::create },
-    { "gazebo",    Gazebo::create },
+    { "crrcsim",            CRRCSim::create },
-    { "last_letter", last_letter::create },
+    { "jsbsim",             JSBSim::create },
-    { "tracker",   Tracker::create },
+    { "gazebo",             Gazebo::create },
-    { "balloon",   Balloon::create }
+    { "last_letter",        last_letter::create },
+    { "tracker",            Tracker::create },
+    { "balloon",            Balloon::create }
 };
 
 void SITL_State::_parse_command_line(int argc, char * const argv[])

libraries/SITL/SIM_Aircraft.cpp

@@ -50,6 +50,7 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
     accel_noise(0.3),
     rate_hz(400),
     autotest_dir(NULL),
+    frame(frame_str),
 #ifdef __CYGWIN__
     min_sleep_time(20000)
 #else

libraries/SITL/SIM_Aircraft.h

@@ -98,6 +98,7 @@ protected:
     uint64_t last_wall_time_us;
     uint8_t instance;
     const char *autotest_dir;
+    const char *frame;
 
     bool on_ground(const Vector3f &pos) const;
 

libraries/SITL/SIM_Helicopter.cpp

@@ -47,24 +47,66 @@ Helicopter::Helicopter(const char *home_str, const char *frame_str) :
  */
 void Helicopter::update(const struct sitl_input &input)
 {
-    float swash1 = (input.servos[0]-1000) / 1000.0f;
-    float swash2 = (input.servos[1]-1000) / 1000.0f;
-    float swash3 = (input.servos[2]-1000) / 1000.0f;
-    float tail_rotor = (input.servos[3]-1000) / 1000.0f;
-    float rsc = (input.servos[7]-1000) / 1000.0f;
-
     // how much time has passed?
     float delta_time = frame_time_us * 1.0e-6f;
 
-    float thrust = (rsc/rsc_setpoint)*(swash1+swash2+swash3)/3.0f;
+    float rsc = (input.servos[7]-1000) / 1000.0f;
-
-    // very simplistic mapping to body euler rates
-    float roll_rate = swash1 - swash2;
-    float pitch_rate = (swash1 + swash2)/2.0f - swash3;
-    float yaw_rate = tail_rotor - 0.5f;
-
     float rsc_scale = rsc/rsc_setpoint;
 
+    float thrust = 0;
+    float roll_rate = 0;
+    float pitch_rate = 0;
+    float yaw_rate = 0;
+    float torque_effect_accel = 0;
+    float lateral_x_thrust = 0;
+    float lateral_y_thrust = 0;
+
+    float swash1 = (input.servos[0]-1000) / 1000.0f;
+    float swash2 = (input.servos[1]-1000) / 1000.0f;
+    float swash3 = (input.servos[2]-1000) / 1000.0f;
+
+    if (strcmp(frame, "heli") == 0) {
+        // simulate a traditional helicopter
+
+        float tail_rotor = (input.servos[3]-1000) / 1000.0f;
+    
+        thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f;
+        torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel;
+
+        roll_rate = swash1 - swash2;
+        pitch_rate = (swash1+swash2) / 2.0f - swash3;
+        yaw_rate = tail_rotor - 0.5f;
+  
+        lateral_y_thrust = yaw_rate * rsc_scale * tail_thrust_scale;
+    } else if (strcmp(frame, "heli-dual") == 0) {
+        // simulate a tandem helicopter
+
+        float swash4 = (input.servos[3]-1000) / 1000.0f;
+        float swash5 = (input.servos[4]-1000) / 1000.0f;
+        float swash6 = (input.servos[5]-1000) / 1000.0f;
+        
+        thrust = (rsc / rsc_setpoint) * (swash1+swash2+swash3+swash4+swash5+swash6) / 6.0f;
+        torque_effect_accel = (rsc_scale + rsc / rsc_setpoint) * rotor_rot_accel * ((swash1+swash2+swash3) - (swash4+swash5+swash6));
+
+        roll_rate = (swash1-swash2) + (swash4-swash5);
+        pitch_rate = (swash1+swash2+swash3) - (swash4+swash5+swash6);
+        yaw_rate = (swash1-swash2) + (swash5-swash4);
+    } else if (strcmp(frame, "heli-compound") == 0) {
+        // simulate a compound helicopter
+
+        float right_rotor = (input.servos[3]-1000) / 1000.0f;
+        float left_rotor = (input.servos[4]-1000) / 1000.0f;
+    
+        thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f;
+        torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel;
+
+        roll_rate = swash1 - swash2;
+        pitch_rate = (swash1+swash2) / 2.0f - swash3;
+        yaw_rate = right_rotor - left_rotor;
+
+        lateral_x_thrust = (left_rotor+right_rotor-1) * rsc_scale * tail_thrust_scale;
+    }
+
     roll_rate *= rsc_scale;
     pitch_rate *= rsc_scale;
     yaw_rate *= rsc_scale;
@@ -81,7 +123,7 @@ void Helicopter::update(const struct sitl_input &input)
     rot_accel.z -= gyro.z * radians(400.0)  / terminal_rotation_rate;
 
     // torque effect on tail
-    rot_accel.z += (rsc_scale+thrust) * rotor_rot_accel;
+    rot_accel.z += torque_effect_accel;
 
     // update rotational rates in body frame
     gyro += rot_accel * delta_time;
@@ -96,7 +138,7 @@ void Helicopter::update(const struct sitl_input &input)
     // scale thrust to newtons
     thrust *= thrust_scale;
 
-    accel_body = Vector3f(0, yaw_rate * rsc_scale * tail_thrust_scale, -thrust / mass);
+    accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass);
     Vector3f accel_earth = dcm * accel_body;
     accel_earth += Vector3f(0, 0, GRAVITY_MSS);
     accel_earth += air_resistance;