Quadcopter drifts heavily in Position Hold mode

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?

Our problem got solved when we shifted to the stable firmware version 1.9.0. I suspect that since the EKF2 filter uses data from the SLAM package, yaw estimate from IMU and the raw IMU data to calculate XY estimates, and since there was no problem with the yaw estimates as far as we could see (altitude hold mode was working absolutely fine), and SLAM package had proved to work fine in the past, the problem could have been with the particular part of the EKF2 filter itself which calculates the XY estimates, which is why changing the firmware fixed it. I hope this helps anyone who faced the same problem!

1 Like