Calibrar el sensor MPU6050 desde cualquier inclinación

Buenas a toda la comunidad,

El proyecto en el que estoy trabajando incluye un módulo MPU6050. Con este mido la orientación del sensor leyendo los valores YPR que me ofrece la librería MPU6050. Uso el ejemplo MPU6050_DMP6.ino para esta consulta.

Quiero que el sensor se pueda calibrar a zero (yaw=0, pitch=0, roll=0) en cualquier orientación del módulo cuando yo se lo indique. Es decir, que apretando un botón, vea en el serial los valores del YPR a 0 esté como esté el módulo, y que cuando mueva el sensor me aparezcan los valores YPR calibrados.

He encontrado este tópico en GigHub que intenta resolver el problema con estas dos funciones:

        mpu.CalibrateAccel(6);
        mpu.CalibrateGyro(6);

Pero no consigue dar valores correctos. Parece que estas funciones solo sirvan para usarse cuando el sensor está totalmente horizontal.

También he intentado usar una matriz de rotación para poner a 0 los valores de YPR. Pero esto no es posible ya que el YPR depende del vector de gravedad, como se observa en el siguiente código:

//Guarda los valores Euler en el array euler[]
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);

//Guarda los valores YPR en el array ypr[]
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

Mi pregunta es si hay alguna manera de calibrar el sensor MPU6050 desde cualquier orientación siempre que quiera.

¡Gracias de antemano!