From 98f87f508723bce18e51739dddf22ff729192225 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 19 Jul 2018 18:23:36 +1000 Subject: [PATCH] SITL: fixed gimbal for tailsitter --- libraries/SITL/SIM_Gimbal.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/libraries/SITL/SIM_Gimbal.cpp b/libraries/SITL/SIM_Gimbal.cpp index 29947c20d8..8c582a5db6 100644 --- a/libraries/SITL/SIM_Gimbal.cpp +++ b/libraries/SITL/SIM_Gimbal.cpp @@ -57,10 +57,8 @@ void Gimbal::update(void) Matrix3f vehicle_dcm; fdm.quaternion.rotation_matrix(vehicle_dcm); - Vector3f vehicle_gyro = Vector3f(radians(fdm.rollRate), - radians(fdm.pitchRate), - radians(fdm.yawRate)); - Vector3f vehicle_accel_body = Vector3f(fdm.xAccel, fdm.yAccel, fdm.zAccel); + const Vector3f &vehicle_gyro = AP::ins().get_gyro(); + const Vector3f &vehicle_accel_body = AP::ins().get_accel(); // take a copy of the demanded rates to bypass the limiter function for testing Vector3f demRateRaw = demanded_angular_rate;