roll = atan2(fYa, sqrt(fXa * fXa + fZa * fZa));
pitch = atan2(fXa, sqrt(fYa * fYa + fZa * fZa));
One of the two equations for pitch and roll is wrong, so it is not surprising that the correction is wrong. I did not check further. Here is a reference to one possible set of correct functions to use in tilt compensation.