Skip to content

gz: fix gimbal yaw, add dds publisher #25754

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 4 commits into from
Oct 14, 2025
Merged

gz: fix gimbal yaw, add dds publisher #25754

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
2fe486e
gz: correct gimbal yaw
dakejahl Oct 14, 2025
5d84a96
uxrce_dds: add publisher /fmu/out/gimbal_device_attitude_status
dakejahl Oct 14, 2025
9118533
chore: use explicit ENU_to_NED rotation
beniaminopozzan Oct 14, 2025
5cd5187
format
dakejahl Oct 14, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions msg/GimbalDeviceAttitudeStatus.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,9 @@ uint16 DEVICE_FLAGS_NEUTRAL = 2
uint16 DEVICE_FLAGS_ROLL_LOCK = 4
uint16 DEVICE_FLAGS_PITCH_LOCK = 8
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 angular_velocity_x
Expand Down
10 changes: 6 additions & 4 deletions src/modules/simulation/gz_bridge/GZGimbal.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -100,11 +100,15 @@ void GZGimbal::gimbalIMUCallback(const gz::msgs::IMU &IMU_data)
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_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(),
IMU_data.orientation().x(),
IMU_data.orientation().y(),
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(),
IMU_data.angular_velocity().y(),
Expand Down Expand Up @@ -206,13 +210,11 @@ void GZGimbal::publishDeviceInfo()

void GZGimbal::publishDeviceAttitude()
{
// TODO handle flags

gimbal_device_attitude_status_s gimbal_att{};

gimbal_att.target_system = 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);
gimbal_att.angular_velocity_x = _gimbal_rate[0];
gimbal_att.angular_velocity_y = _gimbal_rate[1];
Expand Down
4 changes: 4 additions & 0 deletions src/modules/uxrce_dds_client/dds_topics.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,6 +104,10 @@ publications:
type: px4_msgs::msg::Wind
rate_limit: 1.

- topic: /fmu/out/gimbal_device_attitude_status
type: px4_msgs::msg::GimbalDeviceAttitudeStatus
rate_limit: 20.

# Create uORB::Publication
subscriptions:
- topic: /fmu/in/register_ext_component_request
Expand Down
Loading