Compute yaw from magnetometer and accelerometer
I don't know how all your sensor's axis are lined up, but it looks like you should level the magnetometer readings by rotating them by -pitch and -roll to get back to the global reference frame, instead of rotating the magnetometer readings by the positive angles.

However, The pitch and roll angles in the code are both relative the horizon, not a set of euler angles that define a rotation between two states; because of this, they do not compose to rotate the magnetometer back into the global xy frame like I assume the equations are trying to do. The code is also only using atan for pitch and roll instead of atan2, so their range is only 180 degrees.

You need to make sure the rotation matrix you multiply your magnetometer reading by is based on the same conventions as the equations you use to get pitch and roll from the accelerometer, and that it cancels the observed rotation.

To do this correctly, I recommend you read into euler angles. You might also try searching around for a good library that handles 9-DOF sensor fusion for you.