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

SITL: update tailsitter ground behavoir

Browse Source
This commit is contained in:
Iampete1 2021-10-22 23:57:06 +01:00 committed by Andrew Tridgell
parent 1e09e79617
commit ef9580fcf2
1 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/SITL/SIM_Aircraft.cpp
View File

@ -690,15 +690,24 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
break;
}
case GROUND_BEHAVIOR_TAILSITTER: {
// point straight up
// rotate normal refernce frame to get yaw angle, then rotate back
Matrix3f rot;
rot.from_rotation(ROTATION_PITCH_270);
float r, p, y;
dcm.to_euler(&r, &p, &y);
(dcm * rot).to_euler(&r, &p, &y);
y = y + yaw_rate * delta_time;
dcm.from_euler(0.0f, radians(90), y);
dcm.from_euler(0.0, 0.0, y);
rot.from_rotation(ROTATION_PITCH_90);
dcm *= rot;
// X, Y movement tracks ground movement
velocity_ef.x = gnd_movement.x;
velocity_ef.y = gnd_movement.y;
if (velocity_ef.z > 0.0f) {
velocity_ef.z = 0.0f;
}
gyro.zero();
gyro.x = yaw_rate;
use_smoothing = true;
break;
}
}