El signo de cada término de la ecuación puede variar en función de cómo tengáis orientada la IMU en vuestros drone o de la disposición de los motores. Es importante asegurar que los signos están bien puestos para evitar accidentes catastróficos el primer día de vuelo. Para ello, es imprescindible montar nuestro MPU6050 en el sentido que indico en esta entrada. Una vez hecho esto, recomiendo situar el drone en el suelo e ir inclinándolo manualmente en sus tres ejes mientras visualizamos las señales esc1, esc2, esc3 y esc4 para comprobar que todo esté en orden.

La estrategia de control completa representada en bloques quedaría de la siguiente forma. Únicamente programando esto nuestro drone podría volar sin problemas.

Pasemos a entender cómo programamos todo esto en Arduino. Partiremos asumiendo que ya hemos leído y procesado los datos del mando RC y del sensor MPU6050 en Arduino. Primero vamos a analizar todas las partes por separado, y finalmente pondré el código completo.

En primer lugar hay que hacer que el lazo de control se ejecute de forma constante exactamente cada 6ms (en mi caso). Hacer esto en Arduino es extremadamente simple:

float tiempo_ejecucion, loop_timer;

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

void loop() {

  //
  // PROGRAMA PRINCIPAL AQUÍ
  //

  while (micros() - loop_timer < 6000);
  tiempo_ejecucion = (micros() - loop_timer) / 1000;
  loop_timer = micros();

  //
  // O AQUÍ
  //

  Serial.println(tiempo_ejecucion);
}

Como veis, es muy fácil de entender, simplemente hacemos los cálculos pertinentes y esperamos sin hacer nada hasta que pasen 6ms. Más adelante veremos cómo aprovechar estos tiempos muertos donde Arduino no hace nada (solo espera) para ejecutar pequeñas acciones que no necesitan de mucha carga de procesamiento, como puede ser medir la tensión de la batería o gestionar algún led. De esta forma nos aseguramos de que aunque el tiempo de ejecución del lazo de control varíe por lo que sea (mas adelante en este apartado veremos porque), siempre se ejecutará una vez exactamente cada 6ms. El resto del tiempo estará esperando a llegar al tiempo fijado.

** El tiempo de ejecución no tiene por qué ser de 6ms, pueden ser de 4ms, 10ms o el valor que sea, vosotros elegís el valor que queráis utilizar, a mi con 6ms me va bien. Evidentemente cuanto más rápido sea el control, mayor estabilidad tendrá nuestro drone.

El siguiente paso es programar los PID. Como ya he dicho, sobre este tema se han escrito ríos de tinta, por lo que no voy a entrar a explicarlo una vez más. Como veis, es bastante simple de entender, simplemente calculamos en error como hemos hecho más arriba, y lo multiplicamos por los valores de Kp, Ki y Kd. Partiendo de que ya dominamos el tema del sensor MPU6050 y la lectura de las variables recibidas desde el mando, vamos a ver como se programan los famosos PID utilizando Arduino. Este código no es ejecutable como tal, es simplemente para que veías como se programa y entendíais el código. Intentad entender en código y la función de los tres parámetros del PID:

//===================================================== PID PITCH w
pitch_w_error_prop = PIDwInPitch - gyro_pitch_input;  // Calculamos el error
ITerm_pitch_w += (Ki_pitch_w * pitch_w_error_prop);  
ITerm_pitch_w = constrain(ITerm_pitch_w, salidaPI_pitch_w_min1, salidaPI_pitch_w_max1);
DPitch_w = Kd_pitch_w * (gyro_pitch_input - pitch_w_giroscopio_anterior);

PID_pitch_w = Kp_pitch_w * pitch_w_error_prop + ITerm_pitch_w - DPitch_w; // salida PID
PID_pitch_w = constrain(PID_pitch_w, salidaPI_pitch_w_min2, salidaPI_pitch_w_max2);
pitch_w_giroscopio_anterior = gyro_pitch_input;

//=============================================== PID ROLL w
roll_w_error_prop = PIDwInRoll - gyro_roll_input;
ITerm_roll_w += (Ki_roll_w * roll_w_error_prop);
ITerm_roll_w = constrain(ITerm_roll_w, salidaPI_roll_w_min1, salidaPI_roll_w_max1);
DRoll_w = Kd_roll_w * (gyro_roll_input - roll_w_giroscopio_anterior);

PID_roll_w = Kp_roll_w * roll_w_error_prop + ITerm_roll_w - DRoll_w; // salida PID
PID_roll_w = constrain(PID_roll_w, salidaPI_roll_w_min2, salidaPI_roll_w_max2);
roll_w_giroscopio_anterior = gyro_roll_input;

//================================================= PID YAW w
yaw_w_error_prop = wYawConsigna - gyro_yaw_input;
ITerm_yaw_w += (Ki_yaw_w * yaw_w_error_prop);
ITerm_yaw_w = constrain(ITerm_yaw_w, salidaPI_yaw_w_min1, salidaPI_yaw_w_max1);
DYaw_w = Kd_yaw_w * (gyro_yaw_input - yaw_w_giroscopio_anterior);

PID_yaw_w = Kp_yaw_w * yaw_w_error_prop + ITerm_yaw_w - DYaw_w; // salida PID
PID_yaw_w = constrain(PID_yaw_w, salidaPI_yaw_w_min2, salidaPI_yaw_w_max2);
yaw_w_giroscopio_anterior = gyro_yaw_input;

El siguiente paso es generar las señales PWM para los motores en función las salidas de los controladores PID (recordad que los motores se gobiernan con salidas PWM). ¿Por qué no utilizar la función milliseconds() que ya incorpora Arduino? Esto generaría una señal PWM del ancho de pulso que indiquemos entre paréntesis. Pues la respuesta es bien sencilla: porque estas salidas PWM se ejecutan a 50Hz (cada 20ms), y ¿de qué serviría ejecutar el lazo de control cada 6ms, si luego esa información no puede transmitirse a los motores hasta pasados 20ms? Para ello, tenemos que hacer que la frecuencia de las salidas PWM de nuestra placa Arduino sea la misma que la de nuestro lazo de control, 6ms, de esta forma logramos que salida de los PID se aplique sin ningún retraso.

Vamos a analizar la siguiente imagen donde resumo el funcionamiento del lazo de control y la generacion PWM. Todos los ciclos comienzan con las cuatro señales PWM en estado HIGH, siempre. La duración del pulso es calculada por los controladores PID en el ciclo anterior. Cuando la señal paso a estado LOW, se hace la lectura del sensor MPU6050, la lectura del mando, y el cálculo de los PID del periodo siguiente. En el ejemplo de la imagen inferior, el primer cálculo arroja como resultado 1.6ms de PWM, que se aplica al pulso del siguiente ciclo.:

El tiempo que el pulso está en estado HIGH puede variar entre 1ms (parado) y 2ms (máxima velocidad). Esto hace que para que cada periodo dure exactamente 6ms, el tiempo de espera varíe en la misma proporción, de ahí la importancia de controlar el tiempo de ejecución.

Para generar las salidas PWM y que estas estén sincronizadas con la frecuencia de ejecución del lazo de control vamos a utilizar el siguiente código. Al principio puede que resulte un tanto complicado de entender, pero en cuento lo tengáis, comprenderéis que es una solución muy simple. Lo primero que hacemos al comienzo de cada nuevo periodo de 6ms es poner las 4 salidas PWM en estado HIGH:

// ============================================== Modulador
esc1 = PotenciaFilt + PID_pitch_w - PID_roll_w - PID_yaw_w; // Motor 1
esc2 = PotenciaFilt + PID_pitch_w + PID_roll_w + PID_yaw_w; // Motor 2
esc3 = PotenciaFilt - PID_pitch_w + PID_roll_w - PID_yaw_w; // Motor 3
esc4 = PotenciaFilt - PID_pitch_w - PID_roll_w + PID_yaw_w; // Motor 4

// ============================================== PWMs a HIGH
digitalWrite(3, HIGH); //Motor 1 HIGH
digitalWrite(4, HIGH); //Motor 2 HIGH
digitalWrite(5, HIGH); //Motor 3 HIGH
digitalWrite(6, HIGH); //Motor 4 HIGH

Después, sumamos al tiempo calculado por el modulador (señales esc1, esc2, esc3 y esc4) el tiempo total transcurrido desde el inicio del programa (loop_timer). Por ejemplo, si el modulador calcula un tiempo de PWM de 1.6ms para el motor número 1, y desde que hemos inicial el programa han transcurrido 1s (1000ms):

accion_m1 = 1000ms + 1.6ms = 1001.6ms

accion_m1 = esc1 + loop_timer;
accion_m2 = esc2 + loop_timer;
accion_m3 = esc3 + loop_timer;
accion_m4 = esc4 + loop_timer;

Finalmente, utilizando el siguiente bucle while, pasamos a estado LOW las cuatro señales PWM, cada una cuando corresponda. Cuando la variable esc_loop_timer (el tiempo real transcurrido) sea igual a la variable , significará que el pulso ha estado en estado HIGH exactamente 1.6ms, por lo que es hora de pasarlo a estado LOW:

while (aM1 || aM2 || aM3 || aM4 == true) {
  esc_loop_timer = micros();

  if (accion_m1 <= esc_loop_timer) { // Motor LOW
    aM1 = false;
    digitalWrite(3, LOW);

    if (accion_m2 <= esc_loop_timer) { // Motor 2 LOW
      aM2 = false;
      digitalWrite(4, LOW);
    }

    if (accion_m3 <= esc_loop_timer) { // Motor 3 LOW
      aM3 = false;
      digitalWrite(5, LOW);
    }

    if (accion_m4 <= esc_loop_timer) { // Motor 4 LOW
      aM4 = false;
      digitalWrite(6, LOW);
    }
  }
}

Arduino no saldrá de este bucle while hasta que las cuatro señales PWM estén en estado LOW. ¡Ya tenemos señales PWM de frecuencia personalizada!

Una vez que las cuatro señales PWM están en estado HIGH, sabemos que tenemos un margen de tiempo de 1ms (que es el ancho de pulso mínimo para las señales PWM) donde Arduino no va a hacer nada, solo esperar. Podemos aprovechar este mili-segundo para hacer alguna pequeña tarea que para Arduino suponga un tiempo de procesado menor a 1ms. Yo, por ejemplo, utilizo este tiempo para leer la tensión de la batería y activar la correspondiente alarma en caso de que la tensión sea demasiado baja:

Realmente el tiempo de espera de Arduino varía entre 1ms (parado) y 2ms (máxima potencia) dependiendo del estado de los motores. Aun así siempre hay que asegurarse de que la tarea que realizamos en este tiempo muerto sea inferior a 1ms, en caso contrario no será posible controlar los motores a bajas velocidades ya que el pulso no pasará a estado LOW hasta haber acabado esta tarea. Imaginemos que en el tiempo muerto realizamos una tarea que requiere 1.5ms de tiempo de ejecución, ¡el ancho de pulso mínimo de las señales PWM que conseguiríamos sería de 1.5ms! los motores nunca llegarían a detenerse por completo, cosa muy peligrosa.

Como hemos visto, el modo acrobático se basa únicamente en las lecturas de velocidad angular obtenidas del sensor MPU6050. Cuando giramos alguna de las palancas de nuestro mando, estamos ordenando al drone que gire en una determinada dirección a una determinada velocidad, por lo que al soltar la palanca el drone se mantendrá inclinando en el punto donde lo hayamos dejado, no volverá a su posición inicial a 0º de inclinación, solo se habrá detenido la rotación (0º/s). Imaginemos que giramos la palanca de pitch y el drone empieza a gira sobre su eje a una determinada velocidad. Cuando soltemos la palanca y esta vuelva a su posición inicial, el drone recibirá la nueva consigan de velocidad 0, es decir, que se quede quieto en su posición, por lo que quedará inclinado. Para que el drone vuelva a su posición inicial de 0º de inclinación, habrá que girar la palanca en sentido contrario y mandar una consigna de velocidad negativa hasta que quede nivelado y podamos soltar la palanca. El modo acrobático es muy difícil de controlar, únicamente los pilotos experimentados puede hacerlo forma segura. Para nosotros es un paso intermedio antes de conseguir el drone estable que veremos en el siguiente apartado.

¿Veis? ¡No es tan complicado! Acudid al apartado donde os dejo el software completo y dedicadle algo de tiempo a entender bien todos estos conceptos. Si habéis llegado hasta aquí entendiéndolo todo, ya tenéis en 80% hecho. Vayamos a por el modo estable.

Modo de control ‘Estable’

Este modo necesita de dos PID en cascada por cada eje a controlar, además de lecturas de velocidad de rotación y aceleración del sensor MPU6050 a partir de los cuales calcular el ángulo de inclinación de cada eje en grados (º). La ventaja de este modo de vuelo es que el drone es completamente estable y por lo tanto mucho más fácil de manejar. Al contrario que en el modo acrobático, cuando soltemos alguna de las palancas del mando RC el drone volverá automáticamente a su posición de 0º de inclinación. La consigna que mandamos desde el mando es de grados de inclinación (º), no de velocidad (º/s) como en el caso acrobático.

El funcionamiento de la estrategia de control es muy simple. Si detectamos una inclinación, ordenamos al drone que gire en dirección contraria a una velocidad determinada hasta contrarrestar esta inclinación:

Secuencia: Esta secuencia se ejecuta cada 5ms (200Hz)

• Arduino pide lecturas de velocidad angular y aceleración en los tres ejes al sensor MPU6050.
• El sensor responde con sus lecturas y se realizan los cálculos de velocidad angular (º/s) e inclinación (º).
• Con las lecturas de inclinación y las consignas que recibimos desde el mando, el primer PID calcula el error de inclinación y genera la consigna de velocidad para contrarrestarla.
• El segundo PID toma esta consigna del lazo exterior y con la lectura de velocidad de la del sensor MPU6050, genera la salida en microsegundos para enviar a los motores. Los motores aceleran y contrarrestan la desviación de velocidad, que a la vez está contrarrestando la desviación en la inclinación.
• Vuelta al paso 1)

**Si hay interrupción hardware: Leer mando RC → Cada 20ms aprox. recibimos una lectura.

Lo primero que hace esta estructura de control es comparar la consigna de inclinación (º) que nos llega desde el mando, en la imagen ‘Pitch* (mando)’, con la lectura que recibimos del sensor MPU6050, en la imagen ‘Pitch (IMU)’. Fijaos en que en este modo de vuelo el mando fija la consigna de inclinación (º) y no de velocidad de rotación (º/s) como en el modo acrobático. Haciendo la resta de estas dos señales conseguimos en valor de desviación o error en nuestro eje pitch, en la imagen, señal Err.1. Esta variable se da en grados de inclinación (º), 5º, 10º… la que sea:

Esta variable Err.1 pasa por el primer PID (PID_angulo o PID estable) y genera una salida que se utilizará como referencia para el siguiente lazo o lazo de velocidad (el utilizado en el modo acrobático). Lo que conseguimos con esto es indicar al drone que si está inclinado, tiene que aumentar/reducir la velocidad en los motores y rotar a una determinada velocidad y en dirección contaría a la inclinación para contrarrestarla y volver a la posición inicial de 0º:

Como hemos dicho, se compara la consigna que nos llega desde el lazo estable, en la imagen ‘w* pitch’, con la lectura de velocidad de rotación que recibimos del sensor MPU6050, en la imagen ‘w Pitch (IMU)’, para generar el error ‘Err.2’. Este error representa la desviación entre la velocidad que necesitamos para contrarrestar la inclinación y la velocidad real de rotación del drone. Finalmente la variable Err.2 pasa por el PID de velocidad (PID_w) y generamos la salida para el modulador que actuará sobre los motores.

Cuando el drone comience a girar y se vaya corrigiendo la desviación, tanto las variables Err.1 y Err 2 irán disminuyendo hasta convertirse en 0, momento en el que drone habrá vuelto a su posición inicial y no haya desviación alguna entre la consigna que enviamos desde el mando y la inclinación real.

La siguiente figura muestra la estrategia de control total utilizada en el modo estable representada con bloques. Es parecida a la figura mostrada para el modo acrobático, solo que utilizando un PID más en los ejes pitch y roll (el eje yaw no requiere de otro PID). El primer PID toma la lectura de inclinación (º) calculada a partir de las lecturas del sensor MPU6050 y la compara con la consigna del mando. Si hay una desviación de inclinacion, este primer PID genera una referencia de velocidad para el siguiente lazo, acelerando los correspondientes motores y contrarrestando la inclinación. El segundo PID controla la velocidad a que que rota el drone mientras contrarresta la inclinación. Es una estrategia bastante intuitiva:

Utilizando este método conseguiremos que al soltar la palanca del mando, el drone vuelva automáticamente a su posición inicial de 0º sin tener que mandar una consigna de velocidad negativa para contrarrestar la inclinación. El funcionamiento de los dos métodos es evidente, el acrobático  simplemente compensa la rotación (º/s), mientras que el estable compensa la inclinación (º): 

En el apartado ‘código completo’ podéis descargaros el software completo que utilizo yo. Utilizad esta entrada para entender el código.

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 (código).
4. MPU6050 y su programación en Arduino (código).
5. Batería LiPo (código).
6. Control de estabilidad y PID.
7. → Motores, ESC y su programación en Arduino (código).
8. Calibración de hélices y motores (código).
9. Software completo y esquema detallado (código).
10. Como leer variables de Arduino en Matlab (código).
11. Los mejores drones de 2018 | Comparativa y guía de compra.