diff --git a/src/modules/simulation/gz_bridge/GZGimbal.cpp b/src/modules/simulation/gz_bridge/GZGimbal.cpp index f5bc4881b75..01534509663 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.cpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.cpp @@ -84,8 +84,10 @@ void GZGimbal::Run() publishJointCommand(_gimbal_yaw_cmd_publisher, _yaw_stp, _yaw_rate_stp, _last_yaw_stp, _yaw_min, _yaw_max, dt); } - if (_mnt_mode_out == 2) { - // We have a Mavlink gimbal capable of sending messages + if (_mnt_mode_out == 2 && _gimbal_present) { + // We have a Mavlink gimbal capable of sending messages. Only present it if the + // model actually has a gimbal (_gimbal_present), otherwise PX4 would advertise a + // phantom gimbal device and could not manage an external MAVLink gimbal. publishDeviceInfo(); publishDeviceAttitude(); } @@ -102,6 +104,9 @@ void GZGimbal::gimbalIMUCallback(const gz::msgs::IMU &IMU_data) { pthread_mutex_lock(&_node_mutex); + // Receiving gimbal IMU data means the model actually has a gimbal. + _gimbal_present = true; + 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); diff --git a/src/modules/simulation/gz_bridge/GZGimbal.hpp b/src/modules/simulation/gz_bridge/GZGimbal.hpp index a2a7965094a..8b3d92ae564 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.hpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.hpp @@ -80,6 +80,11 @@ private: matrix::Quatf _q_gimbal = matrix::Quatf(1.0f, 0.0f, 0.0f, 0.0f); float _gimbal_rate[3] = {NAN}; + // Set once we receive gimbal IMU data from Gazebo, i.e. the model actually has a + // gimbal. Until then we don't present a MAVLink gimbal device, so that a model + // without a gimbal lets PX4 act as a manager for an external MAVLink gimbal instead. + bool _gimbal_present = false; + // Device information attributes const char _vendor_name[32] = "PX4"; const char _model_name[32] = "Gazebo Gimbal";