From 0966398d8deb972ddc568b2f1d5cb3600e437955 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 3 Jan 2016 12:30:51 +1100
Subject: [PATCH] SITL: improved realism of fixed wing sim somewhat

still not good, but a bit better for manual flight
---
 libraries/SITL/SIM_Plane.cpp     | 16 ++++++++--------
 libraries/SITL/SIM_Plane.h       | 14 ++++++++------
 libraries/SITL/SIM_QuadPlane.cpp |  3 ++-
 3 files changed, 18 insertions(+), 15 deletions(-)

diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp
index 0215d52161..35b60c6082 100644
--- a/libraries/SITL/SIM_Plane.cpp
+++ b/libraries/SITL/SIM_Plane.cpp
@@ -71,7 +71,7 @@ float Plane::calculate_drag_induced(void) const
     if (airspeed < 0.1) {
         return 0;
     }
-    float drag_i = sq(lift) / (0.25 * sq(air_density) * sq(airspeed) * wing_area * M_PI_F * wing_efficiency * aspect_ratio);
+    float drag_i = coefficient.lift_drag * sq(lift) / (0.25 * sq(air_density) * sq(airspeed) * wing_area * M_PI_F * wing_efficiency * aspect_ratio);
     return drag_i;
 }
 
@@ -124,25 +124,25 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel
     // add torque of stabilisers
     rot_accel.z += velocity_bf.y * speed_scaling * coefficient.vertical_stabiliser;
     rot_accel.y -= velocity_bf.z * speed_scaling * coefficient.horizontal_stabiliser;
+
+    // calculate angle of attack
+    angle_of_attack = atan2f(velocity_bf.z, velocity_bf.x);
+    beta = atan2f(velocity_bf.y,velocity_bf.x);
+    
+    // add dihedral
+    rot_accel.x -= beta * airspeed * coefficient.dihedral;
     
     // velocity in body frame
     velocity_bf = dcm.transposed() * velocity_ef;
 
-    // calculate angle of attack
-    angle_of_attack = atan2f(velocity_bf.z, velocity_bf.x);
-    
     // get lift and drag in body frame, in neutons
     Vector3f lift_drag = calculate_lift_drag();
     
-    // air resistance
-    Vector3f air_resistance = -velocity_ef * (GRAVITY_MSS/terminal_velocity);
-
     // scale thrust to newtons
     thrust *= thrust_scale;
 
     accel_body = Vector3f(thrust/mass, 0, 0);
     accel_body += lift_drag/mass;
-    accel_body += dcm.transposed() * air_resistance;    
 
     // add some noise
     add_noise(thrust / thrust_scale);
diff --git a/libraries/SITL/SIM_Plane.h b/libraries/SITL/SIM_Plane.h
index 7bcf669759..6bf3e533eb 100644
--- a/libraries/SITL/SIM_Plane.h
+++ b/libraries/SITL/SIM_Plane.h
@@ -39,10 +39,9 @@ public:
     }
 
 protected:
-    const float hover_throttle = 0.5f;
+    const float hover_throttle = 1.2f;
     const float cruise_airspeed = 20;
     const float cruise_pitch = radians(4);
-    const float terminal_velocity = 55;
     const float wing_efficiency = 0.9;
     const float wing_span = 2.0;
     const float wing_chord = 0.15;
@@ -50,18 +49,21 @@ protected:
     const float wing_area = wing_span * wing_chord;
     const float air_density = 1.225; // kg/m^3 at sea level, ISA conditions
     float angle_of_attack;
+    float beta;
     Vector3f velocity_bf;
 
     // manually tweaked coefficients. Not even close to reality
     struct {
-        float drag = 0.001;
-        float lift = 3.0;
+        float drag = 0.005;
+        float lift = 2.0;
+        float lift_drag = 0.5;
         float vertical_stabiliser = 0.1;
-        float horizontal_stabiliser = 0.001;
+        float horizontal_stabiliser = 2;
+        float dihedral = 0.1;
     } coefficient;
 
     float thrust_scale;
-    Vector3f terminal_rotation_rate{radians(360), radians(360), radians(180)};
+    Vector3f terminal_rotation_rate{radians(170), radians(200), radians(180)};
     Vector3f max_rates{radians(350), radians(250), radians(100)};
 
     float calculate_lift(void) const;
diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp
index 70d39ead2b..ec2d3a3eaf 100644
--- a/libraries/SITL/SIM_QuadPlane.cpp
+++ b/libraries/SITL/SIM_QuadPlane.cpp
@@ -37,7 +37,8 @@ QuadPlane::QuadPlane(const char *home_str, const char *frame_str) :
     Plane(home_str, frame_str)
 {
     frame = &quad_frame;
-    frame->init(mass, 0.51, 50, 20*radians(360));
+    // we use a very high terminal velocity to let the plane model handle the drag
+    frame->init(mass, 0.51, 100, 20*radians(360));
 }
 
 /*