Getting gimbal position, Typhoon H480

Boris_Gubanov · December 22, 2022, 6:58am

I am using ROS Noetic and running a Gazebo with a Typhoon H480 model.
I need to get the angles of gimbal. I found three topics that seem to be useful to me:
/mavros/mount_control/command
/mavros/mount_control/orientation
/mavros/mount_control/status
But when I listen to topics “/mavros/mount_control/orientation” and “/mavros/mount_control/status”, there are no messages there, instead I get:
WARNING: no messages received and simulated time is active.
Is /clock being published?

What am I doing wrong? How do I get the gimbal angles?

JulianOes · December 22, 2022, 7:34pm

I think the MAVLink message that you should expect is MOUNT_ORIENTATION. Can you check if you see that one coming in? E.g. using QGC MAVLink inspector.

Boris_Gubanov · December 22, 2022, 9:36pm

The most similar thing I found was GIMBAL_DEVICE_ATTITUDE_STATUS. But I don’t understand at all how I can get its values in ROS

JulianOes · December 22, 2022, 10:56pm

Ok, this means the new gimbal v2 protocol is supported, which is great. I’m the author of that protocol but I forgot that we ported it to Typhoon H480. What sort of version of PX4 are you running on the Typhoon H480?

And this explains why it doesn’t show up in MAVROS. I assume MAVROS does not support gimbal v2 yet.

Boris_Gubanov · December 23, 2022, 4:23am

I use v1.13.1.
What do you advise me to do?

Switch to an older version of autopilot (which one?)
Wait for protocol support in ROS (is work underway now?)
Maybe I can somehow change the protocol to an older one using QGC

JulianOes · December 24, 2022, 5:18am

Oh, you’re talking SITL, not the real Typhoon H480. Sorry I was confused. It’s time I take a break over Christmas .

So, I suggest you switch to the gimbal v1 protocol. In theory you should be able to change the protocol which is used between MAVROS (and QGC) and PX4 only but does not change the way how PX4 talks to the Gazebo gimbal plugin.

For this, you would change the param MNT_MODE_IN to 3, and then you restart SITL. (Or you change it in the startup script if you want it to stick.

And maybe make an issue for MAVROS to raise awareness regarding gimbal v1 vs. v2.

More docs are here: Gimbal Protocol v2 · MAVLink Developer Guide

Boris_Gubanov · December 24, 2022, 8:07pm

Unfortunately, it didn’t help. I set MNT_MODE_IN to 3: MAVLINK_DO_MOUNT (protocol v1, to be deprecated) and rebooted.
But message MOUNT_ORIENTATION did not appear, message GIMBAL_DEVICE_ATTITUDE_STATUS remained.

I will definitely make an issue for MAVROS

JulianOes · December 25, 2022, 10:56pm

GIMBAL_DEVICE_ATTITUDE_STATUS is still sent because the gimbal itself sends it.

Now, MOUNT_ORIENTATION probably needs to be requested or started manually. For the onboard mode it should be sent automatically, see here:

github.com

PX4/PX4-Autopilot/blob/54d825730ea81ff63532552f562a9b63aa66237b/src/modules/mavlink/mavlink_main.cpp#L1556


      
          		// Note: streams requiring low latency come first
          		configure_stream_local("TIMESYNC", 10.0f);
          		configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
          		configure_stream_local("HIGHRES_IMU", 50.0f);
          		configure_stream_local("LOCAL_POSITION_NED", 30.0f);
          		configure_stream_local("ATTITUDE", 100.0f);
          		configure_stream_local("ALTITUDE", 10.0f);
          		configure_stream_local("DISTANCE_SENSOR", 10.0f);
          		configure_stream_local("ESC_INFO", 10.0f);
          		configure_stream_local("ESC_STATUS", 10.0f);
          		configure_stream_local("MOUNT_ORIENTATION", 10.0f);
          		configure_stream_local("OBSTACLE_DISTANCE", 10.0f);
          		configure_stream_local("ODOMETRY", 30.0f);
          
          
		configure_stream_local("ACTUATOR_CONTROL_TARGET0", 10.0f);
          		configure_stream_local("ADSB_VEHICLE", unlimited_rate);
          		configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
          		configure_stream_local("ATTITUDE_TARGET", 10.0f);
          		configure_stream_local("BATTERY_STATUS", 0.5f);
          		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
          		configure_stream_local("COLLISION", unlimited_rate);

And onboard is what is used on the MAVLink instance on UDP port 14540, see:

github.com

PX4/PX4-Autopilot/blob/main/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink#L26


      
          mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local
          mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local
          mavlink stream -r 50 -s ATTITUDE -u $udp_gcs_port_local
          mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_gcs_port_local
          mavlink stream -r 50 -s ATTITUDE_TARGET -u $udp_gcs_port_local
          mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u $udp_gcs_port_local
          mavlink stream -r 20 -s RC_CHANNELS -u $udp_gcs_port_local
          mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local
          
          
# API/Offboard link
          mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_offboard_port_remote
          
          
# Onboard link to camera
          mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote
          
          
# Onboard link to gimbal
          mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal -o $udp_onboard_gimbal_port_remote
          
          
# To display for SIH sitl
          if [ "$PX4_SIMULATOR" = "sihsim" ]; then
          	udp_sihsim_port_local=$((19450+px4_instance))

Is that the one you’re using?

If you’re using 14550 instead, then it might not be sent by default and you have to request it.

Then the next question is if the gimbal module actually publishes MOUNT_ORIENTATION.
The publish method is there: PX4-Autopilot/output.cpp at fe80e7aa468a50bec6b035d0e8e4e37e516c84ff · PX4/PX4-Autopilot · GitHub

And then here is your answer why it doesn’t work:

github.com

PX4/PX4-Autopilot/blob/fe80e7aa468a50bec6b035d0e8e4e37e516c84ff/src/modules/gimbal/gimbal.cpp#L281-L285


      
          			// Only publish the mount orientation if the mode is not mavlink v1 or v2
          			// If the gimbal speaks mavlink it publishes its own orientation.
          			if (params.mnt_mode_out != 1 && params.mnt_mode_out != 2) { // 1 = MAVLink v1, 2 = MAVLink v2
          				thread_data.output_obj->publish();
          			}

I think you need to change that part to make it always send for you.