Hi all (especially Bill & Doug)
I wonder why "analog" offsetting is done for the gyros and accelerometers at program start? Why not filling omega_I with appropriate startup values instead? This way the code gets a bit leaner and kind of legacy-free.
Do me the favor:
Set all AN_OFFSETs to zero right before main loop.
Set Omega_I to 6, Omega_I to -6, and Omega_I to 6 right before main loop (tested for Razor 9DOF). No levelling is required anymore to start with proper readouts. Ki_yaw may be set a bit larger.