From e06faf5ea686e67eb3774d4ff0c8feb22d9f3ab0 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Wed, 23 Oct 2019 01:46:17 +0200 Subject: [PATCH] Vehicle: rotate ATTITUDE_QUATERNION when repr_offset_q is available --- src/Vehicle/Vehicle.cc | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 2412e85b1..55eee91ea 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -12,6 +12,8 @@ #include #include +#include + #include "Vehicle.h" #include "MAVLinkProtocol.h" #include "FirmwarePluginManager.h" @@ -1132,15 +1134,25 @@ void Vehicle::_handleAttitudeQuaternion(mavlink_message_t& message) mavlink_attitude_quaternion_t attitudeQuaternion; mavlink_msg_attitude_quaternion_decode(&message, &attitudeQuaternion); + Eigen::Quaternionf quat(attitudeQuaternion.q1, attitudeQuaternion.q2, attitudeQuaternion.q3, attitudeQuaternion.q4); + Eigen::Vector3f rates(attitudeQuaternion.rollspeed, attitudeQuaternion.pitchspeed, attitudeQuaternion.yawspeed); + Eigen::Quaternionf repr_offset(attitudeQuaternion.repr_offset_q[0], attitudeQuaternion.repr_offset_q[1], attitudeQuaternion.repr_offset_q[2], attitudeQuaternion.repr_offset_q[3]); + + // if repr_offset is valid, rotate attitude and rates + if (repr_offset.norm() >= 0.5f) { + quat = quat * repr_offset; + rates = repr_offset * rates; + } + float roll, pitch, yaw; - float q[] = { attitudeQuaternion.q1, attitudeQuaternion.q2, attitudeQuaternion.q3, attitudeQuaternion.q4 }; + float q[] = { quat.w(), quat.x(), quat.y(), quat.z() }; mavlink_quaternion_to_euler(q, &roll, &pitch, &yaw); _handleAttitudeWorker(roll, pitch, yaw); - rollRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.rollspeed)); - pitchRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.pitchspeed)); - yawRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.yawspeed)); + rollRate()->setRawValue(qRadiansToDegrees(rates[0])); + pitchRate()->setRawValue(qRadiansToDegrees(rates[1])); + yawRate()->setRawValue(qRadiansToDegrees(rates[2])); } void Vehicle::_handleGpsRawInt(mavlink_message_t& message) -- 2.22.0