Messing with AHRS parameters. Also added wrapper functions for AHRS and IMU functions from Madgwick's code