omega_I applied continuously. _ki larger. Stop integrating when _omega.length()>20
The key change was the scaling of ge to ensure the error is not
quadratic
This makes 3 major changes:
1) fixes the scaling of the yaw drift correction term to fix the time
constant
2) don't integrate the mag vector over multiple readings
3) accumulate omega_I changes over 15 seconds before applying, to try
to prevent omega_I picking up short term responses
this buffers up _omega_I changes in _omega_I_sum over a period of 10
seconds, applying the slope limit only when _omega_I_sum is
transferred to _omega_I.
The result is a huge improvement in the ability of _omega_I to track
gyro drift over the long term.
when a user first connects with USB, and later switches to the
telemetry port without restarting we were getting zero for error_yaw
in the logs, as AHRS.get_error_yaw() was being called twice.
This ensures we give the last value after the counter is reset
