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.
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_bodypitch_body and yaw_bodyare 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.
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 :
