Stable simulated flight controls

Reduced ahrs accel and mag gains to .01 and .001.
Implemented gyroscope offset because the gyroscope error was high enough to be greater than the accel and mag corrections because the gains were reduced to so small.
Implemented on startup, rapid AHRS convergence.
Added a bunch of junk code for testing ahrs performance.