working averaged gyro code with negligible drift