Added Gyro functionality. Maths for Accel and Gyro still need to be chekced.