Adding gimbal rate in gz simulation (#24125)

* Adding gimbal rate in gz simulation * add submodule
PX4 · Dec 18, 2024 · 8a93913 · 8a93913
1 parent 6b10f1c
commit 8a93913
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 1 deletion.
diff --git a/Tools/simulation/gz b/Tools/simulation/gz
diff --git 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);
 }