optimizing gyro calibration

calibration works correctly for mpu6050, ITG3200 might need some testing