/* Calibrate the gyro data in the DMP. After eight seconds of no motion, the DMP will compute gyro biases and subtract them from the quaternion output. If  */
int dmp_enable_gyro_cal(unsigned char enable)
/* Calibrate the gyro data in the DMP. After eight seconds of no motion, the DMP will compute gyro biases and subtract them from the quaternion output. If  */
int dmp_enable_gyro_cal(unsigned char enable)
{
    if (enable) {
        unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d};
        return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
    } else {
        unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7};
        return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
    }
}