AHRS updates, finished code, probably lots of bugs in there at this point