Magnetometer dynamic calibration
I like your graphs. They clearly show that roll, pitch, and yaw seem to be working.
Congratulations! That's already more progress than most people make.

I'm guessing that the code you presented is calculating "the wrong" MAG_Heading value, different from the MAG_Heading value you expected.

It would be a lot easier for us to help you if you gave us:
(This is the "describe the symptoms" section of
"How To Ask Questions The Smart Way"
)


the AK8975 magnetometer output values m_x, m_y, and m_z at some single point in time.
The pitch and roll values at the same instant
the allegedly wrong MAG_Heading output value calculated from those values
what you expected the correct MAG_Heading to be


So I'm left to speculate that perhaps you're running into the same sorts of problems I create for myself :-).


What angle format do your sin() and cos() and atan2() functions expect? Do you need to do some sort of conversion between the format pitch and roll are stored in to that format? Do you need to convert from that format to what you need MAG_heading? (brads, degrees, or radians? floating-point or fixed-point?)
Is there an offset in the raw m_x, m_y, m_z values that needs to be subtracted off?
Are all the parts lined up in the way assumed by the code? In particular, is the pitch and roll axis lined up with the magnetometer axis? (Is m_x supposed to point forward, along the roll axis? Is m_y supposed to point to the right, along the pitch axis?)
Maybe some sensor value or another -- perhaps m_z -- needs to be negated before feeding into this code?
Is maybe this code being interrupted by one interrupt or another that corrupts its internal values? I seem to recall a different project that, after someone put a "divide" in an interrupt routine, every trig function calculation everywhere else in the program would often give the wrong result.
Is maybe interrupts firing so often that this code never actually finishes running?


There seems to be other people discussing very similar code elsewhere:
http://diydrones.com/forum/topics/heading-from-3d-magnetometer ;
http://diydrones.ning.com/profiles/blogs/dcm-imu-theory-first-draft ;
http://aeroquad.com/showthread.php?1138-REVOLUTION!!!-New-IMU!!! ;
http://www.rcgroups.com/forums/showthread.php?t=1436742&amp;page=6 ;
http://aeroquad.com/showthread.php?691-Hold-your-heading-with-HMC5843-Magnetometer ;
etc.