Moved compass to IMU, reworked code. Not yet tested