Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: Make soaring thermals slanted.

This commit is contained in:
Samuel Tabor 2019-06-07 07:40:44 +01:00 committed by Andrew Tridgell
parent a18d6e925d
commit ae27f96361
1 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/SITL/SIM_Aircraft.cpp
View File

@ -831,11 +831,17 @@ float Aircraft::get_local_updraft(Vector3f currentPos)
break;
}
// Wind drift at this altitude
float driftX = sitl->wind_speed * currentPos.z * cosf(sitl->wind_direction * DEG_TO_RAD);
float driftY = sitl->wind_speed * currentPos.z * sinf(sitl->wind_direction * DEG_TO_RAD);
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 thermalPos(thermals_x[iThermal] + driftX/thermals_w[iThermal],
thermals_y[iThermal] + driftY/thermals_w[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]));