aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Samuel Tabor 2019-05-26 10:56:10 +01:00 committed by Andrew Tridgell
parent 394fbb1a50
commit 5082c9be8b
8 changed files with 109 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Tools/autotest/ArduPlane-Missions/CMAC-soar.txt Normal file
View File

@ -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

30
Tools/autotest/default_params/plane-soaring.parm Normal file
View File

@ -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

4
Tools/autotest/pysim/vehicleinfo.py
View File

@ -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",

52
libraries/SITL/SIM_Aircraft.cpp
View File

@ -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) {

3
libraries/SITL/SIM_Aircraft.h
View File

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

5
libraries/SITL/SIM_Plane.cpp
View File

@ -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;
}
}
/*

4
libraries/SITL/SITL.cpp
View File

@ -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
};

2
libraries/SITL/SITL.h
View File

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