Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2025-12-06 17:26:32 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

gz: fix gimbal yaw, add dds publisher (#25754)

Browse Source 
* gz: correct gimbal yaw

* uxrce_dds: add publisher /fmu/out/gimbal_device_attitude_status

* chore: use explicit ENU_to_NED rotation

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>

* format

---------

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>
Co-authored-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>
This commit is contained in:
Jacob Dahl
2025-10-14 11:45:43 -08:00
committed by GitHub
parent babe094d06
commit a64536802b
3 changed files with 13 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
msg/GimbalDeviceAttitudeStatus.msg
View File

@@ -9,6 +9,9 @@ uint16 DEVICE_FLAGS_NEUTRAL = 2
uint16 DEVICE_FLAGS_ROLL_LOCK = 4 uint16 DEVICE_FLAGS_ROLL_LOCK = 4
uint16 DEVICE_FLAGS_PITCH_LOCK = 8 uint16 DEVICE_FLAGS_PITCH_LOCK = 8
uint16 DEVICE_FLAGS_YAW_LOCK = 16 uint16 DEVICE_FLAGS_YAW_LOCK = 16
uint16 DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME = 32
uint16 DEVICE_FLAGS_YAW_IN_EARTH_FRAME = 64
float32[4] q float32[4] q
float32 angular_velocity_x float32 angular_velocity_x

10
src/modules/simulation/gz_bridge/GZGimbal.cpp
View File

@@ -100,11 +100,15 @@ void GZGimbal::gimbalIMUCallback(const gz::msgs::IMU &IMU_data)
pthread_mutex_lock(&_node_mutex); pthread_mutex_lock(&_node_mutex);
static const matrix::Quatf q_FLU_to_FRD = matrix::Quatf(0.0f, 1.0f, 0.0f, 0.0f); static const matrix::Quatf q_FLU_to_FRD = matrix::Quatf(0.0f, 1.0f, 0.0f, 0.0f);
static const matrix::Quatf q_ENU_to_NED = matrix::Quatf(0.0f, cosf(M_PI_4_F), cosf(M_PI_4_F), 0.0f);
// Get the gimbal orientation. Gimbal frame is FLU in Gazebo, reference frame is ENU in Gazebo
const matrix::Quatf q_gimbal_FLU = matrix::Quatf(IMU_data.orientation().w(), const matrix::Quatf q_gimbal_FLU = matrix::Quatf(IMU_data.orientation().w(),
IMU_data.orientation().x(), IMU_data.orientation().x(),
IMU_data.orientation().y(), IMU_data.orientation().y(),
IMU_data.orientation().z()); IMU_data.orientation().z());
_q_gimbal = q_FLU_to_FRD * q_gimbal_FLU * q_FLU_to_FRD.inversed();
_q_gimbal = q_ENU_to_NED * q_gimbal_FLU * q_FLU_to_FRD.inversed();
matrix::Vector3f rate = q_FLU_to_FRD.rotateVector(matrix::Vector3f(IMU_data.angular_velocity().x(), matrix::Vector3f rate = q_FLU_to_FRD.rotateVector(matrix::Vector3f(IMU_data.angular_velocity().x(),
IMU_data.angular_velocity().y(), IMU_data.angular_velocity().y(),
@@ -206,13 +210,11 @@ void GZGimbal::publishDeviceInfo()
void GZGimbal::publishDeviceAttitude() void GZGimbal::publishDeviceAttitude()
{ {
// TODO handle flags
gimbal_device_attitude_status_s gimbal_att{}; gimbal_device_attitude_status_s gimbal_att{};
gimbal_att.target_system = 0; // Broadcast gimbal_att.target_system = 0; // Broadcast
gimbal_att.target_component = 0; // Broadcast gimbal_att.target_component = 0; // Broadcast
gimbal_att.device_flags = 0; gimbal_att.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME;
_q_gimbal.copyTo(gimbal_att.q); _q_gimbal.copyTo(gimbal_att.q);
gimbal_att.angular_velocity_x = _gimbal_rate[0]; gimbal_att.angular_velocity_x = _gimbal_rate[0];
gimbal_att.angular_velocity_y = _gimbal_rate[1]; gimbal_att.angular_velocity_y = _gimbal_rate[1];

4
src/modules/uxrce_dds_client/dds_topics.yaml
View File

@@ -104,6 +104,10 @@ publications:
type: px4_msgs::msg::Wind type: px4_msgs::msg::Wind
rate_limit: 1. rate_limit: 1.
- topic: /fmu/out/gimbal_device_attitude_status
type: px4_msgs::msg::GimbalDeviceAttitudeStatus
rate_limit: 20.
# Create uORB::Publication # Create uORB::Publication
subscriptions: subscriptions:
- topic: /fmu/in/register_ext_component_request - topic: /fmu/in/register_ext_component_request