Offboard mode AttitudeSetpoint failure over ROS2 and FastRTPS

[deb0ch]

Hey,

We are trying to control a drone from ROS2 over FastRTPS using the topic VehicleAttitudeSetPoint_PubSubTopic, but our drone is failing miserably the instant we switch to OFFBOARD mode as seen in the attached video.

Taking your advice @tsc21 (from this thread) we looked at how the MAVLink message is handled and in addition went to look in Mavros how the different flags are set to reproduce the exact same sequences as when the mavlink messages are sent from Mavros. We also took the bit from the px4_ros_com offboard example for timestamp synchronisation.

But whenever we switch to OFFBOARD mode, setpoints for the body rate controllers stop being sent (you can see the red curve stutter in the video) and the drone crashes. I would have expected to still have an attitude controller running and sending setpoints to the underlying controlers, but for some reason this is not happening even after reproducing the exact same behavior (in terms of uORB topics) as when a mavlink message is sent by Mavros and received by PX4.

I also saw that other people faced the same issue without finding answers. @Andrew_Saba did you find a solution to your problem ?

What did we miss ?

• px4_ros_com version: 958ccdb
• PX4-Autopilot version : 378b41a

Please find in this public Google Drive :

• A video showing the problem
• The ulog files for this flight
• The code used during this flight (spidrone_control.py, spidrone_control.yaml)

https://review.px4.io/plot_app?log=bd61e22e-3ec1-477c-ba27-4bebd95d7156

[Jaeyoung-Lim]

@deb0ch Looking at your code, you are setting the attitude via roll_body pitch_body yaw_body. However these fields are used for body rates.

But then you specify that you are ignoring body rate setpoints and enable attitude setpoints. So you have conflicting masks with data

[deb0ch]

Hello @Jaeyoung-Lim, first thank you for your swift answer !

I looked in more details and it seems to me that there is more to it than your first answer.

Firstly, you are talking about masks so I want to make sure that there is no confusion: neither Mavlink nor Mavros are involved in my example, I am only using FastRTPS publishing directly to uORB topics, and the VehicleAttitudeSetpoint does not have the type_mask field that the Mavlink message has.

I also find it highly unlikely that the fields that I am using are for angular rates :

• From the comments in the code, the fields that I am using seem to be documented for setpoints of attitude and not angular rates.
• Moreover, if you look at mavlink_receiver.cpp, you can see that attitude and angular rate setpoints are sent in two different steps and topics: here and here.

However, when I read the code again after your response I saw that there is indeed a step that I am not doing the same which is filling up the quaternion field ; seeing both euler and quaternion being documented for the same use led me to assume that you could use either the one or the other. Notice that the roll_body pitch_body and yaw_body are filled with the same angles as the ones held by the q_d quaternion, so seeing that I doubt even more that these should hold an attitude rate.

I will get back to you whenever we have tried filling this quaternion field as well.

[Bastien_Taillandier]

Hi there,

I have news about this issue.
As @deb0ch said, I filled the quaternion field and remove VehicleCommand stuff.

I just send throttle command and when I switch to offboard mode the drone react correctly to the command.

However, the failsafe error message appear :

I have the feeling that this error message hasn’t effect on the behavior of the drone ? Am I wrong ?
Why this failsafe appear and how can I correct this ?

I also desactived failsafe for the offboard mode but nothing apparently change :

Here my code :
https://drive.google.com/file/d/1Lba_nBi9GFLq2BvnvH8Si9rJPvuSRMCj/view?usp=sharing

[rroche]

Just make sure the COM_OBL_RC_ACT param is set to -1

http://docs.px4.io/master/en/advanced_config/parameter_reference.html#COM_OBL_RC_ACT

I’ll take a look at your code to check-out what’s going on.

[Himanshu_Vairagade]

Can you please share the Updated code ? @Bastien_Taillandier