SITL: Add plane-soaring type, including parameters, mission, simulated thermals and glider sim settings.

Tools/autotest/ArduPlane-Missions/CMAC-soar.txt

@@ -0,0 +1,9 @@
+QGC WPL 110
+0	1	0	16	0.000000	0.000000	0.000000	0.000000	-35.362938	149.165085	584.409973	1
+1	0	3	22	15.000000	0.000000	0.000000	0.000000	-35.361164	149.163986	28.110001	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.359467	149.161697	400.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366333	149.162659	400.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366131	149.164581	400.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.359272	149.163757	400.000000	1
+6	0	3	177	2.000000	-1.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+7	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.359272	149.163757	400.000000	1

Tools/autotest/default_params/plane-soaring.parm

@@ -0,0 +1,30 @@
+ARSPD_FBW_MAX   20
+ARSPD_FBW_MIN   6
+
+PTCH2SRV_I       0.500000
+PTCH2SRV_IMAX    4000.000000
+
+SIM_THML_SCENARI 1
+
+SOAR_ALT_CUTOFF  150.000000
+SOAR_ALT_MAX     350.000000
+SOAR_ALT_MIN     50.000000
+SOAR_DIST_AHEAD  5.000000
+SOAR_ENABLE      1.000000
+SOAR_ENABLE_CH   0.000000
+SOAR_MIN_CRSE_S  10.000000
+SOAR_MIN_THML_S  20.000000
+SOAR_POLAR_B     0.031000
+SOAR_POLAR_CD0   0.027000
+SOAR_POLAR_K     25.600000
+SOAR_Q1          0.001000
+SOAR_Q2          0.030000
+SOAR_R           0.450000
+SOAR_VSPEED      0.700000
+
+TECS_SPDWEIGHT   2.000000
+
+TRIM_ARSPD_CM    900.000000
+
+WP_LOITER_RAD    10.000000
+

Tools/autotest/pysim/vehicleinfo.py

@@ -220,6 +220,10 @@ class VehicleInfo(object):
                 "waf_target": "bin/arduplane",
                 "default_params_filename": ["default_params/plane.parm", "default_params/plane-dspoilers.parm"]
             },
+            "plane-soaring": {
+                "waf_target": "bin/arduplane",
+                "default_params_filename": ["default_params/plane.parm", "default_params/plane-soaring.parm"]
+            },
             "gazebo-zephyr": {
                 "waf_target": "bin/arduplane",
                 "default_params_filename": "default_params/gazebo-zephyr.parm",

libraries/SITL/SIM_Aircraft.cpp

@@ -578,6 +578,8 @@ void Aircraft::update_wind(const struct sitl_input &input)
                        sinf(radians(input.wind.direction))*cosf(radians(input.wind.dir_z)), 
                        sinf(radians(input.wind.dir_z))) * input.wind.speed;
 
+    wind_ef.z += get_local_updraft(position);
+
     const float wind_turb = input.wind.turbulence * 10.0f;  // scale input.wind.turbulence to match standard deviation when using iir_coef=0.98
     const float iir_coef = 0.98f;  // filtering high frequencies from turbulence
 
@@ -792,6 +794,56 @@ void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel)
     }
 }
 
+float Aircraft::get_local_updraft(Vector3f currentPos)
+{
+    int scenario = sitl->thermal_scenario;
+
+    #define MAX_THERMALS 10
+
+    float thermals_w[MAX_THERMALS];
+    float thermals_r[MAX_THERMALS];
+    float thermals_x[MAX_THERMALS];
+    float thermals_y[MAX_THERMALS];
+
+    int n_thermals = 0;
+
+    switch (scenario) {
+        case 1:
+            n_thermals = 1;
+            thermals_w[0] =  2.0;
+            thermals_r[0] =  80.0;
+            thermals_x[0] = -180.0;
+            thermals_y[0] = -260.0;
+            break;
+        case 2:
+            n_thermals = 1;
+            thermals_w[0] =  4.0;
+            thermals_r[0] =  30.0;
+            thermals_x[0] = -180.0;
+            thermals_y[0] = -260.0;
+            break;
+        case 3:
+            n_thermals = 1;
+            thermals_w[0] =  2.0;
+            thermals_r[0] =  30.0;
+            thermals_x[0] = -180.0;
+            thermals_y[0] = -260.0;
+            break;
+    }
+
+    int iThermal;
+    float w = 0.0f;
+    float r2;
+    for (iThermal=0;iThermal<n_thermals;iThermal++) {
+        Vector3f thermalPos(thermals_x[iThermal],thermals_y[iThermal],0);
+        Vector3f relVec = currentPos - thermalPos;
+        r2 = relVec.x*relVec.x + relVec.y*relVec.y;
+        w += thermals_w[iThermal]*exp(-r2/(thermals_r[iThermal]*thermals_r[iThermal]));
+    }
+
+    return w;
+}
+
 void Aircraft::add_twist_forces(Vector3f &rot_accel)
 {
     if (sitl == nullptr) {

libraries/SITL/SIM_Aircraft.h

@@ -269,6 +269,9 @@ protected:
     void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel);
     void add_twist_forces(Vector3f &rot_accel);
 
+    // get local thermal updraft
+    float get_local_updraft(Vector3f currentPos);
+
 private:
     uint64_t last_time_us = 0;
     uint32_t frame_counter = 0;

libraries/SITL/SIM_Plane.cpp

@@ -83,6 +83,11 @@ Plane::Plane(const char *frame_str) :
     if (strstr(frame_str, "-ice")) {
         ice_engine = true;
     }
+
+    if (strstr(frame_str, "-soaring")) {
+        mass = 2.0;
+        coefficient.c_drag_p = 0.05;
+    }
 }
 
 /*

libraries/SITL/SITL.cpp

@@ -229,6 +229,10 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
     AP_GROUPINFO("MAG8_DEVID",    10, SITL, mag_devid[7], 0),
 
     AP_GROUPINFO("LED_LAYOUT",    11, SITL, led_layout, 0),
+
+    // Scenario for thermalling simulation, for soaring
+    AP_GROUPINFO("THML_SCENARI",12, SITL,  thermal_scenario, 0),
+    
     AP_GROUPEND
 
 };

libraries/SITL/SITL.h

@@ -245,6 +245,8 @@ public:
     AP_Float temp_tconst;
     AP_Float temp_baro_factor;
     
+    AP_Int8 thermal_scenario;
+
     // differential pressure sensor tube order
     AP_Int8 arspd_signflip;