Implemented gyro and accel self test

Finally implemented proper self test of gyro and accelerometer based on Invensense MPU-9250 Accelerometer, Gyroscope and Compass Self-Test Implementation Document Number: AN-MPU-9250A-03, Revision: 1.0, Release Date: 5/30/2013. The prcedure is essentially the same as that used for the MPU6050 but the details are a little different.