I have 6DOF camera based on quaternions and I have accumulated roll. I wanted to get rid of it, but cant find the solution.

I know this was discussed much times, but I didnt find any working code for doing this.

For now i have the following code to get rid from unwanted roll rotation:

qRotation *= Quaternion.RotationAxis(Vector3.ForwardLH,
                Vector3.Dot(Vector3.Transform(Vector3.Right, qRotation), Vector3.Up) * 0.04f);
            qRotation = Quaternion.Normalize(qRotation);

But this code leads to unexpected results. First, Camera rotate to 180 degrees on z axis and I cannot rotate it back.

Second - during rotation along X axis, camera start rotating and I change its orientation.

Does someone know how to fix that and do correct auto-leveling?