Warning! Lots of untested code!!! May or may not work properly!!!
Added PID controller for headding (need to fix the fact that the magnetic compass can possible point 180 degrees of if the quadcopter is upside down.)
Added PID controller for acceleration magnetude
made some variables static inside methods
Added a low pass filter for the data. (needs constants!!!)

Most of this functionality is not implemented in the actual loop yet.