Any tips how to make this code more reliable?
Use modern, more reliable and much better code. The best open source AHRS code is state of the art RTIMULib. For one implementation, see this post: https://forum.pololu.com/t/state-of-the-art-ahrs-for-25/8091
The original author is not longer supporting RTIMUlib actively (and as far as I know, it seems to be bug-free) but others have posted branches. It may not support your compass, but that is not a major problem.