Índice:

1. Conceptos generales sobre drones
2. Material necesario y montaje de los componentes hardware
3. Mando RC y receptor. Programación en Arduino
4. → MPU6050 y su programación en Arduino
5. Batería LiPo
6. Control de estabilidad y PID
7. Motores, ESC y su programación en Arduino
8. Calibración de hélices y motores
9. Software completo y esquema detallado
10. Probando el Software completo antes de volar
11. Como leer variables de Arduino en Matlab 
    

El sensor MPU6050

IMU o Inertial Mesurment Module es el sensor más importante del drone y sin el cual es imposible mantener el drone en el aire de forma autónoma. Este pequeño sensor proporciona lecturas de velocidad de rotación (giroscopio) y aceleración en los tres ejes de movimiento (acelerómetro). Procesando adecuadamente estas señales podremos obtener las dos variables imprescindibles para volar el drone: la velocidad de rotación de los tres ejes en grados por segundo (º/s) y la inclinación del drone (º). El sensor que yo utilizo es el MPU6050.

Comprar MPU6050 en Amazon

Código Arduino para lectura del sensor MPU6050

Mas adelante os dejo el código que yo uso para la lectura y procesamiento de los datos del sensor MPU6050, pero en primer lugar vamos a entender las diferentes partes de este código. El primer paso es configurar el sensor. A la hora de hacerlo hay varios parámetros que podemos configurar, como la sensibilidad y escala del sensor o la frecuencia de corte del filtro LPF (low pass filter) que incorpora a la salida. La configuración que he escogido es de 500dps para el giroscopio, +/-8g para el acelerómetro y 98Hz de frecuencia de corte para el filtro de salida. Nos os preocupéis si no entendéis estos conceptos, mas adelante en este post iremos explicándolos mas en detalle:

// Iniciar sensor MPU6050
void MPU6050_iniciar() {
  Wire.beginTransmission(MPU6050_adress);
  Wire.write(0x6B);                        //Registro 6B hex)
  Wire.write(0x00);                        //00000000 para activar giroscopio
  Wire.endTransmission();
  Wire.beginTransmission(MPU6050_adress);
  Wire.write(0x1B);                        //Register 1B hex
  Wire.write(0x08);                        //Giroscopio a 500dps (full scale)
  Wire.endTransmission();
  Wire.beginTransmission(MPU6050_adress);
  Wire.write(0x1C);                        //Register (1A hex)
  Wire.write(0x10);                        //Acelerómetro a  +/- 8g (full scale range)
  Wire.endTransmission();

  Wire.beginTransmission(MPU6050_adress);
  Wire.write(0x1B);
  Wire.endTransmission();
  Wire.requestFrom(MPU6050_adress, 1);
  while (Wire.available() < 1);

  // Activar y configurar filtro pasa bajos LPF que incorpora el sensor
  Wire.beginTransmission(MPU6050_adress);
  Wire.write(0x1A);
  Wire.write(0x04);
  Wire.endTransmission();
}

Si acudimos al datasheet del MPU6050 vemos como estos números que utilizamos en la  configuración tienen perfecto sentido. En primer lugar utilizamos el registro 6B para activar el modo configuración del sensor y poder modificar los parámetros internos:

El registro 1B para configurar el giroscopio:

El registro 1C para configurar el acelerómetro:

Y finalmente el filtro pasa bajos de la salida:

Tened en cuenta que filtrar la salida introduce un retardo en las lecturas del sensor que puede llegar a los 20ms, tened esto en cuenta a la hora de configurar el filtro. En función del nivel de vibraciones que tengamos en el frame después de calibrar los motores y las hélices, habrá que filtrar mas o menos la salida:

Una vez configurado el sensor basta con ir haciendo lecturas periódicas, para lo que utilizaremos las siguientes lineas de código. Simplemente avisamos al sensor de que necesitamos una nueva lectura, y este nos contesta con toda la información:

// Leer sensor MPU6050
void MPU6050_leer() {
  // Los datos del giroscopio y el acelerómetro se encuentran de la dirección 3B a la 14
  Wire.beginTransmission(MPU6050_adress);       // Empezamos comunicación
  Wire.write(0x3B);                             // Pedir el registro 0x3B (AcX)
  Wire.endTransmission();
  Wire.requestFrom(MPU6050_adress, 14);         // Solicitar un total de 14 registros
  while (Wire.available() < 14);                // Esperamos hasta recibir los 14 bytes

  ax = Wire.read() << 8 | Wire.read();          // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  ay = Wire.read() << 8 | Wire.read();          // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  az = Wire.read() << 8 | Wire.read();          // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  temperature = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  gx = Wire.read() << 8 | Wire.read();          // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  gy = Wire.read() << 8 | Wire.read();          // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  gz = Wire.read() << 8 | Wire.read();          // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}

A continuación os dejo el código de lectura de valores raw (valores en crudo y sin procesar) del sensor, podéis ejecutarlo y ver los valores que lee vuestro sensor. Si ejecutamos el código y  visualizamos estos valores raw por el canal serie, veremos cómo estos números tienen poco o ningún sentido para nosotros, pero realmente toda la información que necesitamos está entre esos números, oculta. Toda la información que necesitamos para volar el drone (la velocidad de rotación de los tres ejes en grados por segundo (º/s) y la inclinación del drone (º)) hay que conseguirla procesando estos datos raw. El montaje necesario es muy simple**:

Es muy importante cablear el sensor MPU6050 de forma robusta, asegurando que ninguno de los cables se va a soltar aunque el drone caiga e impacte contra el suelo de forma agresiva. Perdí varios meses intentando averiguar que sucedía con mi drone, ya que el software, tras un impacto, quedaba ‘muerto’, teniendo que resetearlo para poder devolverlo a la vida. El problema era que a raíz de un cableado poco robusto y tras un impacto, el sensor perdía la alimentación durante un breve instante, pero suficiente para apagar y encenderlo. La placa Arduino, que no perdía la alimentación, seguía ejecutando el código de forma normal hasta llegar al comando while(Wire.available() < 14); donde el software quedaba atascado y esperando a recibir la información del sensor MPU6050, información que nunca llegaba debido a que el sensor había sido reseteado por el corte de alimentación y recordad que es necesario inicializarlo tras cada apagado para poder utilizarlo.

La dirección por defecto de todos los sensores MPU6050 que he utilizado a sido siempre 0x68, pero puede pasar que la dirección de vuestro sensor sea diferente. Para saber exactamente cual es la dirección podéis utilizar el siguiente código, simplemente cableáis y alimentáis el sensor como hemos visto mas arriba y ejecutáis el código. En el monitor serie aparecerá la dirección de todos los periféricos I2C que tengáis conectados.

Hecho esto, nos ponemos a programar 😉 Todo el software que utilizaremos a lo largo de esta entrada está subido a GitHub:

Descargar ficheros en GitHub

Código para lectura de valores raw (es necesario instalar la librería Wire.h). Cargad y ejecutad el código. Moved el sensor con la mano y comprobad que los valores cambian. A continuación veremos como entender y procesar toda esta información:

#include <Wire.h>

// MPU6050
#define MPU6050_adress 0x68
int gx, gy, gz;
float ax, ay, az, temperature;

void setup() {
  Wire.begin();
  Serial.begin(115200);

  // Iniciar sensor MPU6050
  MPU6050_iniciar();
}

void loop() {
  // Leer sensor MPU6050
  MPU6050_leer();

  // Monitor Serie
  Serial.print(ax);
  Serial.print("\t");
  Serial.print(ay);
  Serial.print("\t");
  Serial.print(az);
  Serial.print("\t");
  Serial.print(gx);
  Serial.print("\t");
  Serial.print(gy);
  Serial.print("\t");
  Serial.println(gz);
}

// Iniciar sensor MPU6050
void MPU6050_iniciar() {
  Wire.beginTransmission(MPU6050_adress);
  Wire.write(0x6B);                          // PWR_MGMT_1 registro 6B hex
  Wire.write(0x00);                          // 00000000 para activar
  Wire.endTransmission();
  Wire.beginTransmission(MPU6050_adress);
  Wire.write(0x1B);                          // GYRO_CONFIG registro 1B hex
  Wire.write(0x08);                          // 00001000: 500dps
  Wire.endTransmission();
  Wire.beginTransmission(MPU6050_adress);
  Wire.write(0x1C);                          // ACCEL_CONFIG registro 1C hex
  Wire.write(0x10);                          // 00010000: +/- 8g
  Wire.endTransmission();
}

// Leer sensor MPU6050
void MPU6050_leer() {
  // Los datos del giroscopio y el acelerómetro se encuentran de la dirección 3B a la 14
  Wire.beginTransmission(MPU6050_adress);       // Empezamos comunicación
  Wire.write(0x3B);                             // Pedir el registro 0x3B (AcX)
  Wire.endTransmission();
  Wire.requestFrom(MPU6050_adress, 14);         // Solicitar un total de 14 registros
  while (Wire.available() < 14);                // Esperamos hasta recibir los 14 bytes

  ax = Wire.read() << 8 | Wire.read();          // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  ay = Wire.read() << 8 | Wire.read();          // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  az = Wire.read() << 8 | Wire.read();          // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  temperature = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  gx = Wire.read() << 8 | Wire.read();          // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  gy = Wire.read() << 8 | Wire.read();          // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  gz = Wire.read() << 8 | Wire.read();          // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}

Una vez obtenidos los valores raw es necesario procesar esta información de forma adecuada para obtener la velocidad de rotación en grados por segundo (º/s) y la inclinación del drone (º). Para calcular la velocidad de rotación en º/s bastará con utilizar las lecturas de rotación que entrega el sensor y ajustarlas en función de la configuración que hayamos escogido para el giroscopio. Para calcular la inclinación en cambio, habrá que combinar las lecturas del giroscopio y del acelerómetro del sensor y utilizar algunas funciones matemáticas que explicaremos mas adelante.

En primer lugar procesaremos la información obtenida del acelerómetro. La aceleración medida por el sensor dependerá de la sensibilidad que hayamos escogido, en este caso de ±8g. Según la tabla del datasheet del sensor, cada 4096LSB que leamos en raw (en las variables ax, ay, az), equivale a 1g de aceleración, o lo que es lo mismo, a 9.8m/s2. Por ello, dividiendo los valores raw de aceleración entre 4096 obtendremos la aceleración en ‘g‘:

Por lo tanto, y con esta configuración, deberemos leer valores raw cercanos a 0 en los ejes ‘ax’ y ‘ay‘ y valores cercanos  4096 para el eje ‘az’, equivalente a 1g o a 9.8m/s², ya que estaremos midiendo la aceleración terrestre. Nos os preocupéis si al visualizar estas variables os encontrarais con valores diferentes a estos, todos los sensores tienen un offset o error de medida que hará que nunca leamos exactamente 0. El offset lo compensaremos mas adelante calibrando el sensor.

Para la velocidad de rotación en cambio, configurando el sensor para una sensibilidad de ±500dps, cada 65.5LSB de lectura raw (en las variables gx, gy, gz) equivalen a 1º/s de velocidad de rotación:

Por tanto, basta con coger el valor raw de velocidad (gx, gy, gz) que leemos del sensor y dividirlo entre 65.5 para obtener la velocidad de rotación en º/s. Si hubiéramos seleccionado una sensibilidad de ±2000dps, habría que dividir el valor raw entre 16.4. Así de simple, ya tenemos la velocidad de rotación en º/s :

gyro_X = gx / 65.5;
gyro_Y = gy / 65.5;
gyro_Z = gz / 65.5;

Calcular el ángulo de inclinación (º) de cada eje es algo más complicado, pero nada que no se pueda entender dedicándole algo de tiempo. El concepto principal es muy simple, si sabemos la velocidad de rotación y el tiempo que ha estado rotando a esta velocidad, podemos fácilmente calcular los grados que ha rotado en un determinado eje. Por ejemplo, si el drone gira a una velocidad de 10º/s durante 1 segundo, el drone se habrá inclinado 10º. Es como ir con el coche a 100km/h durante una hora, habremos recorrido exactamente 100km. El código necesario es muy simple, basta con multiplicar la velocidad de rotación por el tiempo de ejecución del código:

angulo_pitch += gyro_X * tiempo_ejecucion / 1000;
angulo_roll  += gyro_Y * tiempo_ejecucion / 1000;

Pero no es oro todo lo que reluce. El problema viene debido a un fenómeno llamado drift o deriva en castellano, que aparece siempre que intentamos estimar la inclinación utilizando medidas únicamente del giroscopio. Haced una pequeña buscado por Google y encontraréis infinidad de información sobre este asunto. Yo paso directamente a la solución de este problema, que es complementar el cálculo de la inclinación con medidas del acelerómetro y un filtro complementario. De esta forma compensamos la desviación producida por el efecto drift:

// Filtro complementario
if (set_gyro_angles) {
   angulo_pitch = angulo_pitch * 0.99 + angulo_pitch_acc * 0.01;
   angulo_roll  = angulo_roll  * 0.99 + angulo_roll_acc  * 0.01;
}

Las lecturas del acelerómetro pueden ser extremadamente ruidosas si tenemos vibraciones no deseadas en el drone (por ejemplo si las hélices no están bien calibradas). Este ruido en las mediciones puede generar errores graves en la estimación de la inclinación. Este problema lo atajaremos mas adelante en otro post dedicada al tema de las vibraciones.

Finalmente, conviene siempre calibrar el sensor y eliminar el offset para una estimación de la inclinación lo más precisa posible. Lo que yo hago es calibrar el giroscopio y el acelerómetro de forma separada durante la inicialización del software. Para ello, tomo 3000 muestras tanto del giroscopio como del acelerómetro y calculo el valor medio, que será el offset de nuestro sensor (es imprescindible mantener el sensor los más estático posible durante la calibración). Este método el mucho mas precio que apuntar manualmente los valores de offset, llegando a obtener un error la estimación de la inclinación de únicamente 0.01º con este método.

A continuación os dejo el código completo para que podéis ejecutarlo vosotros mismo:

/*
   Leer sensor MPU6050 (datos raw o sin procesar)
   Más información en https://arduproject.es/mpu6050-y-su-programacion/
*/

#define usCiclo 5000  // Ciclo de ejecución de software en microsegundos
#include <Wire.h>

// MPU6050
#define MPU6050_adress 0x68
float gyro_Z, gyro_X, gyro_Y, temperature, gyro_X_cal, gyro_Y_cal, gyro_Z_cal;
int gx, gy, gz, cal_int;
float acc_total_vector, ax, ay, az;
bool set_gyro_angles, accCalibOK  = false;
float acc_X_cal, acc_Y_cal, acc_Z_cal, angulo_pitch_acc, angulo_roll_acc, angulo_pitch, angulo_roll;

long tiempo_ejecucion, loop_timer;

void setup() {
  Wire.begin();
  Serial.begin(115200);

  // Iniciar sensor MPU6050
  MPU6050_iniciar();

  // Calibrar giroscopio y acelerómetro. El sensor tiene que estar inmovil y en una supercifie plana.
  // Leer los datos del MPU6050 3000 veces y calcular el valor medio
  for (cal_int = 0; cal_int < 3000 ; cal_int ++) {
    MPU6050_leer();   // Leer sensor MPU6050
    gyro_X_cal += gx;
    gyro_Y_cal += gy;
    gyro_Z_cal += gz;
    acc_X_cal  += ax;
    acc_Y_cal  += ay;
    acc_Z_cal  += az;
    delayMicroseconds(50);
  }
  gyro_X_cal = gyro_X_cal / 3000;
  gyro_Y_cal = gyro_Y_cal / 3000;
  gyro_Z_cal = gyro_Z_cal / 3000;
  acc_X_cal  = acc_X_cal  / 3000;
  acc_Y_cal  = acc_Y_cal  / 3000;
  acc_Z_cal  = acc_Z_cal  / 3000;
  accCalibOK = true;

  loop_timer = micros();
}

void loop() {

  // Nuevo ciclo
  while (micros() - loop_timer < usCiclo);
  tiempo_ejecucion = (micros() - loop_timer) / 1000;
  loop_timer = micros();

  MPU6050_leer();     // Leer sensor MPU6050
  MPU6050_procesar(); // Procesar datos del sensor MPU6050

  // Monitor Serie
  Serial.print(angulo_pitch);
  Serial.print("\t");
  Serial.println(angulo_roll);
}

// Iniciar sensor MPU6050
void MPU6050_iniciar() {
  Wire.beginTransmission(MPU6050_adress);
  Wire.write(0x6B);                          // PWR_MGMT_1 registro 6B hex
  Wire.write(0x00);                          // 00000000 para activar
  Wire.endTransmission();
  Wire.beginTransmission(MPU6050_adress);
  Wire.write(0x1B);                          // GYRO_CONFIG registro 1B hex
  Wire.write(0x08);                          // 00001000: 500dps
  Wire.endTransmission();
  Wire.beginTransmission(MPU6050_adress);
  Wire.write(0x1C);                          // ACCEL_CONFIG registro 1C hex
  Wire.write(0x10);                          // 00010000: +/- 8g
  Wire.endTransmission();
}

// Leer sensor MPU6050
void MPU6050_leer() {
  // Los datos del giroscopio y el acelerómetro se encuentran de la dirección 3B a la 14
  Wire.beginTransmission(MPU6050_adress);       // Empezamos comunicación
  Wire.write(0x3B);                             // Pedir el registro 0x3B (AcX)
  Wire.endTransmission();
  Wire.requestFrom(MPU6050_adress, 14);         // Solicitar un total de 14 registros
  while (Wire.available() < 14);                // Esperamos hasta recibir los 14 bytes

  ax = Wire.read() << 8 | Wire.read();          // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  ay = Wire.read() << 8 | Wire.read();          // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  az = Wire.read() << 8 | Wire.read();          // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  temperature = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  gx = Wire.read() << 8 | Wire.read();          // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  gy = Wire.read() << 8 | Wire.read();          // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  gz = Wire.read() << 8 | Wire.read();          // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}

// Cálculo de velocidad angular (º/s) y ángulo (º)
void MPU6050_procesar() {

  // Restar valores de calibración del acelerómetro
  ax -= acc_X_cal;
  ay -= acc_Y_cal;
  az -= acc_Z_cal;
  az  = az + 4096;

  // Restar valores de calibración del giroscopio y calcular
  // velocidad angular en º/s. Leer 65.5 en raw equivale a 1º/s
  gyro_X = (gx - gyro_X_cal) / 65.5;
  gyro_Y = (gy - gyro_Y_cal) / 65.5;
  gyro_Z = (gz - gyro_Z_cal) / 65.5;

  // Calcular ángulo de inclinación con datos del giroscopio
  // 0.000000266 = tiempo_ejecucion / 1000 / 65.5 * PI / 180
  angulo_pitch += gyro_X * tiempo_ejecucion / 1000;
  angulo_roll  += gyro_Y * tiempo_ejecucion / 1000;
  angulo_pitch += angulo_roll  * sin((gz - gyro_Z_cal) * tiempo_ejecucion  * 0.000000266);
  angulo_roll  -= angulo_pitch * sin((gz - gyro_Z_cal) * tiempo_ejecucion  * 0.000000266);

  // Calcular vector de aceleración
  // 57.2958 = Conversion de radianes a grados 180/PI
  acc_total_vector  = sqrt(pow(ay, 2) + pow(ax, 2) + pow(az, 2));
  angulo_pitch_acc  = asin((float)ay / acc_total_vector) * 57.2958;
  angulo_roll_acc   = asin((float)ax / acc_total_vector) * -57.2958;

  // Filtro complementario
  if (set_gyro_angles) {
    angulo_pitch = angulo_pitch * 0.99 + angulo_pitch_acc * 0.01;
    angulo_roll  = angulo_roll  * 0.99 + angulo_roll_acc  * 0.01;
  }
  else {
    angulo_pitch = angulo_pitch_acc;
    angulo_roll  = angulo_roll_acc;
    set_gyro_angles = true;
  }
}

Orientación del sensor en el frame

Como hemos visto en la entrada dedicada al montaje hardware, es necesario montar el sensor MPU6050 en nuestro drone con una orientación determinada. El sensor siempre tiene que estar orientado con uno de los ejes de movimiento, esto es imprescindible. Para ello, orientaremos la ‘fecha Y‘ del sensor con lo que consideremos la ‘parte delantera’ de nuestro drone, de esta forma, alinearemos este eje con nuestro eje Pitch (recordad que inclinarnos en el eje Pitch implica avanzar o retroceder). El eje Roll quedará automáticamente fijado en el eje perpendicular a este eje. Recordad que estamos construyendo un drone tipo ‘x’. Si no tenéis claro cual queréis que sea la parte delantera del drone, seleccionad una al azar y orientad la ‘flecha Y’ en esa dirección:

Ahora, cuando movamos hacia arriba el stick del eje Pitch, el drone se moverá hacia ‘delante’, es decir, hacia en la dirección que indica de ‘flecha Y’ y cuando lo movamos hacia abajo, el drone se moverá hacia ‘atrás’. Lo mismo pasará con el eje Roll, al mover su correspondiente stick a la derecha, nuestro drone rotará en esa dirección.

Continuar con la siguiente entrada:

1. Conceptos generales sobre drones
2. Material necesario y montaje de los componentes hardware
3. Mando RC y receptor. Programación en Arduino
4. MPU6050 y su programación en Arduino
5. → Batería LiPo
6. Control de estabilidad y PID
7. Motores, ESC y su programación en Arduino
8. Calibración de hélices y motores
9. Software completo y esquema detallado
10. Como leer variables de Arduino en Matlab