Changed AHRS code to match Madgwick code on x-io webpage. Reason for change was magnetometer enabled drift on all axis increased to unacceptable levels. Tested with DFROBOT and Sen-10724.