We are currently trying to fly our quadcopter in position hold mode using localization data generated by ORB_SLAM2 package which uses XY data from a Depth Camera (D415), and Z data from “/mavros/altitude” topic. We are publishing the x, y and z data to the “/mavros/vision_position/pose” topic, while we are not setting the orientation data in the message.
Also, we are using an external GPS+Compass module for external magnetometer (although GPS is disabled using GPS_0_CONFIG). We are using Firmware version 1.9.0 (RC). The problem is that no two magnetometers are consistent, so we are able to fly only by disabling three out of the four.
The problem is that as soon as we put the copter into position hold mode, it starts to either roll or pitch constantly in one direction (not just subtly drift in the direction). We need to give a constant oppositely directed roll or pitch input to prevent it from going away. In any case, due to some problem it quickly switches back to altitude hold mode.
Here are sample logs of our flights:
The logs seem to indicate that the XY velocities refuse to follow their setpoints. Also the logs show this weird behaviour where the Local Position X and Y start increasing rapidly and suddenly drop to zero (presumably because Pixhawk started ignoring the XY data received from ORB_SLAM2).
What could be the reason behind this drifting and the other problems? How should we proceed to solve this problem?