From ef9580fcf2a5a1f01b04e0073bd68458c941e5a7 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 22 Oct 2021 23:57:06 +0100 Subject: [PATCH] SITL: update tailsitter ground behavoir --- libraries/SITL/SIM_Aircraft.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index b897694747..ddf6f910fa 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -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; } }