Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: fixed pitch constraint on fixed wing takeoff

Browse Source 
fixes issue with jet takeoff
This commit is contained in:
Andrew Tridgell 2019-07-05 19:17:30 +10:00
parent 5bfb6ced84
commit 626c632859
1 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/SITL/SIM_Aircraft.cpp
View File

@ -554,7 +554,14 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
// zero roll/pitch, but keep yaw // zero roll/pitch, but keep yaw
float r, p, y; float r, p, y;
dcm.to_euler(&r, &p, &y); dcm.to_euler(&r, &p, &y);
dcm.from_euler(0.0f, 0.0f, y); if (velocity_ef.length() < 5) {
// at high speeds don't constrain pitch, otherwise we
// can get stuck in takeoff
p = 0;
} else {
p = MAX(p, 0);
}
dcm.from_euler(0.0f, p, y);
// only fwd movement // only fwd movement
Vector3f v_bf = dcm.transposed() * velocity_ef; Vector3f v_bf = dcm.transposed() * velocity_ef;
v_bf.y = 0.0f; v_bf.y = 0.0f;