Tiempo lectura MPU5060

Buenas,

Estoy utilizando un MPU5060 para un drone. El sketch que estoy utilizando es el siguiente:

SETUP de MPU:

#include <Wire.h>


void setup() {
  //Setup the MPU-6050
  Wire.beginTransmission(gyro_address);                                      //Start communication with the address found during search.
  Wire.write(0x6B);                                                          //We want to write to the PWR_MGMT_1 register (6B hex)
  Wire.write(0x00);                                                          //Set the register bits as 00000000 to activate the gyro
  Wire.endTransmission();                                                    //End the transmission with the gyro.

  Wire.beginTransmission(gyro_address);                                      //Start communication with the address found during search.
  Wire.write(0x1B);                                                          //We want to write to the GYRO_CONFIG register (1B hex)
  Wire.write(0x08);                                                          //Set the register bits as 00001000 (500dps full scale)
  Wire.endTransmission();                                                    //End the transmission with the gyro

  Wire.beginTransmission(gyro_address);                                      //Start communication with the address found during search.
  Wire.write(0x1C);                                                          //We want to write to the ACCEL_CONFIG register (1A hex)
  Wire.write(0x10);                                                          //Set the register bits as 00010000 (+/- 8g full scale range)
  Wire.endTransmission();                                                    //End the transmission with the gyro

  //Let's perform a random register check to see if the values are written correct
  Wire.beginTransmission(gyro_address);                                      //Start communication with the address found during search
  Wire.write(0x1B);                                                          //Start reading @ register 0x1B
  Wire.endTransmission();                                                    //End the transmission
  Wire.requestFrom(gyro_address, 1);                                         //Request 1 bytes from the gyro
  while (Wire.available() < 1);                                              //Wait until the 6 bytes are received
  if (Wire.read() != 0x08) {                                                 //Check if the value is 0x08
    // Error MPU LED??
    while (1)delay(10);                                                        //Stay in this loop for ever
  }

  Wire.beginTransmission(gyro_address);                                      //Start communication with the address found during search
  Wire.write(0x1A);                                                          //We want to write to the CONFIG register (1A hex)
  Wire.write(0x03);                                                          //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
  Wire.endTransmission();                                                    //End the transmission with the gyro
}

LOOP:

void loop() {

  ///// resto del código
  
  tiempo_1 = micros();
  MPU_6050(); // Leer MPU
  tiempo_2 = micros();
  tiempo_MPU = tiempo_2-tiempo_1;

  ///// resto del código

}


void MPU_6050() {
  Wire.beginTransmission(gyro_address);                                  
  Wire.write(0x3B);                                                     
  Wire.endTransmission();                                                
  Wire.requestFrom(gyro_address, 12);                                    

  while (Wire.available() < 12);                                         
  AcX = Wire.read() << 8 | Wire.read();                                  
  AcY = Wire.read() << 8 | Wire.read();                                   
  AcZ = Wire.read() << 8 | Wire.read();                                    
                     
  GyX = Wire.read() << 8 | Wire.read();
  GyY = Wire.read() << 8 | Wire.read();
  GyZ = Wire.read() << 8 | Wire.read();

  if (cal_int == 2000) {   // Calibración
    GyX = GyX - gyro_X_cal;
    GyY = GyY - gyro_Y_cal;
    GyZ = GyZ - gyro_Z_cal;
    AcX = AcX - acc_X_cal;
    AcY = AcY - acc_Y_cal;
    AcZ = AcZ - acc_Z_cal;
  }
  //Calculo de la velocidad angular del Giroscopio
  Gy[0] = GyX / G_R;
  Gy[1] = GyY / G_R;
  Gy[2] = GyZ / G_R;
}

Estoy midiendo el tiempo que tarda en hacer la lectura utilizando las variables tiempo_1, tiempo_2 y tiempo_MPU, y estoy midiendo casi 1.9ms. Mi pregunta es, es normal que en hacer esas lecturas tarde 1900us (1.9ms)? Lo esperaba mucho mas rápido.
Estoy utilizando un arduino Mega. El conexionado es el tipico VCC, GND, I2C.

¿Cuál es el objetivo de esta parte en la función de lectura del MPU6050?

  if (cal_int == 2000) {   // Calibración
    GyX = GyX - gyro_X_cal;
    GyY = GyY - gyro_Y_cal;
    GyZ = GyZ - gyro_Z_cal;
    AcX = AcX - acc_X_cal;
    AcY = AcY - acc_Y_cal;
    AcZ = AcZ - acc_Z_cal;
  }

Ese no puede ser tu código total porque como mínimo cal_int no existe o no esta definido.

No es el código completo. Se trata de un drone asado en Arduino y el código completo es muy extenso y no aporta nada a este problema.

Esta parte solo calibra el MPU en la inicializacion. Podéis omitirlo.

  if (cal_int == 2000) {   // Calibración
    GyX = GyX - gyro_X_cal;
    GyY = GyY - gyro_Y_cal;
    GyZ = GyZ - gyro_Z_cal;
    AcX = AcX - acc_X_cal;
    AcY = AcY - acc_Y_cal;
    AcZ = AcZ - acc_Z_cal;
  }

Mi duda era si esta parte del código (abajo), lectura de valores raw del MPU, es normal que tarde 1.9ms en ejecutarse. Según el datasheet del MPU, el acelerometro tiene una frecuencia de muestreo máxima de 1kH, lo que da aproximadamente 1ms. Supongo que los valores que estoy midiendo son correctos. Aun así, si alguno pudiera confirmarme este punto lo agradecería.

void MPU_6050() {
  Wire.beginTransmission(gyro_address);                                  
  Wire.write(0x3B);                                                     
  Wire.endTransmission();                                                
  Wire.requestFrom(gyro_address, 12);                                    

  while (Wire.available() < 12);                                         
  AcX = Wire.read() << 8 | Wire.read();                                  
  AcY = Wire.read() << 8 | Wire.read();                                   
  AcZ = Wire.read() << 8 | Wire.read();                                    
                     
  GyX = Wire.read() << 8 | Wire.read();
  GyY = Wire.read() << 8 | Wire.read();
  GyZ = Wire.read() << 8 | Wire.read();
}

El “contexto” es lo relevante, aunque a veces creamos parezca que no lo es. Esos 1.9 ms contra el teórico de 1 ms, en la práctica no tendrían que afectar lo que sea que estés controlando.

Lo que afecta es la forma en la que manejas los datos luego de adquirirlos, es posible que en ese “contexto” haya algun delay que te está dando problemas. Mi primer sugerencia es que revises a fondo el código de tu proyecto a la caza de algún delay.

Adicionalmente en la última rutina que nos muestras no parece que apliques algún filtro a los datos, si los usas directamente como salen del MPU tendrás muchas imprecisiones.

Esta es una lectura que te recomiendo para el manejo adecuado de filtros en los datos del MPU6050, no es necesario recurrir a filtros voluminosos, el que sugieren en el link, se puede usar con tiempos de retardo en el ciclo del filtro de hasta 15 ms, incluso se puede usar 0.

Esta es la rutina con el filtro

//Lectura del MPU6050
long previousMillis4 = 0;  long interval4 = 2; 

void MPU6050()
{
 unsigned long currentMillis4 = millis();        
 if(currentMillis4 - previousMillis4 > interval4)
 {  
  previousMillis4 = currentMillis4;
   Wire.beginTransmission(MPU);
   Wire.write(0x3B); //Pedir el registro 0x3B - corresponde al AcX
   Wire.endTransmission(false);
   Wire.requestFrom(MPU,6,true); 
   AcX=Wire.read()<<8|Wire.read();
   AcY=Wire.read()<<8|Wire.read();
   AcZ=Wire.read()<<8|Wire.read();
 
    //Se calculan los angulos Y, X respectivamente.
   Acc[1] = atan(-1*(AcX/A_R)/sqrt(pow((AcY/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
   Acc[0] = atan((AcY/A_R)/sqrt(pow((AcX/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
 
   //Leer los valores del Giroscopio
   Wire.beginTransmission(MPU);
   Wire.write(0x43);
   Wire.endTransmission(false);
   Wire.requestFrom(MPU,4,true);
   GyX=Wire.read()<<8|Wire.read();
   GyY=Wire.read()<<8|Wire.read();
 
   //Calculo del angulo del Giroscopio
   Gy[0] = GyX/G_R;
   Gy[1] = GyY/G_R;
 
   //Filtro complementario asociado a 0-15 ms
   Angle[0] = 0.90 *(Angle[0]+Gy[0]*0.005) + 0.1*Acc[0];
   Angle[1] = 0.90 *(Angle[1]+Gy[1]*0.005) + 0.1*Acc[1];
 }
}