Closed loop controllers for ardupilot

Okay, thanks for the insights, I’ll check the messages and I’ll upload the logs

I had exactly the same problem as you, but i’ve been able to fix it. The thing is that you don’t need to use mavros_controllers, ardupilot has a problem with the attitude topic, so you can send a setpoint position msg (/mavros/setpoint_position/local). This is what i’ve done. Fast planner publishes as (/planning/pos_cmd - PositionCommand), so you can transform it to (/mavros/setpoint_position/local - PoseStamped). I think that the drone crashes because fast planner’s tf is a mess. Looks like that the node takes a world coordinate input but the frame of reference is rotated 90 degrees in relation to map. So, to summarize, you do the transform, normalize the tfs by launching static_transform_publisher, and, if your camera is the same as mine, change the fx and fy constants. But notice that this problem is related to the fast planner node, and nothing related to the PR mentioned or ardupilot.

Hi, thanks for the insights
I know about the 90 degree rotation, we can Mimic that rotation with a transformation or in the sdf you can change add that transformation. And I know that it works in the open loop configuration.

The whole point to work with mavros_controllers is to have a closed loop control, that’s what I’m trying to achieve


Yes, it looks simple, but i think it is not. Not only yaw is rotated 90 degrees in relation to map frame but also camera_link in relation to base_link, also 90 degrees. I had to do two transforms that doesn’t correspond to reality to make it work for me.

yes I agree with you, but after correcting the transforamtions, ideally the mavros_controllers should work. those transformations are not a result of fast-planner, it’s just how iris copter is implemented in PX4 simulation