From 8a9391321dc79f4eea9f3fd97f5ebb172fdb0fdf Mon Sep 17 00:00:00 2001 From: Perre Date: Wed, 18 Dec 2024 16:25:12 +0100 Subject: [PATCH] Adding gimbal rate in gz simulation (#24125) * Adding gimbal rate in gz simulation * add submodule --- Tools/simulation/gz | 2 +- src/modules/simulation/gz_bridge/GZGimbal.cpp | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index f48a17a4b306..75318c05c0c8 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit f48a17a4b30601ebb11e2f9e3e678fec8a73ebdc +Subproject commit 75318c05c0c84d98ac786963d3c333f3bde0d718 diff --git a/src/modules/simulation/gz_bridge/GZGimbal.cpp b/src/modules/simulation/gz_bridge/GZGimbal.cpp index e016098fce8a..976200020e7e 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.cpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.cpp @@ -104,6 +104,14 @@ void GZGimbal::gimbalIMUCallback(const gz::msgs::IMU &IMU_data) IMU_data.orientation().z()); _q_gimbal = q_FLU_to_FRD * q_gimbal_FLU * q_FLU_to_FRD.inversed(); + matrix::Vector3f rate = q_FLU_to_FRD.rotateVector(matrix::Vector3f(IMU_data.angular_velocity().x(), + IMU_data.angular_velocity().y(), + IMU_data.angular_velocity().z())); + + _gimbal_rate[0] = rate(0); + _gimbal_rate[1] = rate(1); + _gimbal_rate[2] = rate(2); + pthread_mutex_unlock(&_node_mutex); }