El signo de cada término de la ecuación puede variar en función de cómo tengáis orientado el sensor MPU6050 en vuestros drone y 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.

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

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

Pasemos a entender cómo programamos todo esto en Arduino. En primer lugar hay que hacer que el lazo de control se ejecute de forma constante exactamente cada 6000μs. 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 6000μs. De esta forma nos aseguramos de que aunque el tiempo de ejecución del lazo de control varíe, siempre se ejecutará una vez cada 6000μs exactamente. El resto del tiempo estará esperando a llegar al tiempo fijado.

Ahora que ya sabemos cómo hacer que el ciclo se ejecute cada 6000μs, veamos como generar con nuestra placa Arduino estas señales PWM para que tengan un periodo de la misma duración que nuestro ciclo de ejecución.

En primer lugar, vamos a asumir que en el ciclo anterior los PID han calculado el tiempo de estas señales PWM en función de la inclinación del drone y de las consigna recibidas del mando radio control, obteniendo ESC1_us microsegundos para el motor 1, ESC2_us microsegundos para el motor 2, ESC3_us microsegundos para el motor 3 y ESC4_us microsegundos para el motor 4:

ESC1_us = RC_Throttle_consigna + PID_W_Pitch_OUT - PID_W_Roll_OUT - PID_W_Yaw_OUT; // Motor 1
ESC2_us = RC_Throttle_consigna + PID_W_Pitch_OUT + PID_W_Roll_OUT + PID_W_Yaw_OUT; // Motor 2
ESC3_us = RC_Throttle_consigna - PID_W_Pitch_OUT + PID_W_Roll_OUT - PID_W_Yaw_OUT; // Motor 3
ESC4_us = RC_Throttle_consigna - PID_W_Pitch_OUT - PID_W_Roll_OUT + PID_W_Yaw_OUT; // Motor 4

Con esta información del ciclo anterior, lo primero que hacemos al comienzo del siguiente ciclo es poner las 4 salidas PWM en estado HIGH (esto mismo puedo verse en la figura superior):

// Para generar las 4 señales PWM, el primer paso es poner estas señales a 1 (HIGH).
digitalWrite(pin_motor1, HIGH);
digitalWrite(pin_motor2, HIGH);
digitalWrite(pin_motor3, HIGH);
digitalWrite(pin_motor4, HIGH);
tiempo_motores_start = micros();

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 utilizar este intervalo para realizar tareas simples como procesar las señales del mando radiocontrol, leer la tensión de batería, o encender algún LED, siempre controlador no sobrepasar el milisegundo de tiempo. 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 este intervalo realizamos una tarea que requiere de 1.5ms para ejecutarse, ¡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.

  // Para generar las 4 señales PWM, el primer paso es poner estas señales a 1 (HIGH).
  digitalWrite(pin_motor1, HIGH);
  digitalWrite(pin_motor2, HIGH);
  digitalWrite(pin_motor3, HIGH);
  digitalWrite(pin_motor4, HIGH);
  tiempo_motores_start = micros();

  // ------------------ ¡¡1ms max!! ------------------
  tiempo_1 = micros();

  RC_procesar();             // Leer mando RC
  LED_blink();               // LED parpadeo
  Lectura_tension_bateria(); // Leer Vbat

  // Si la duracion entre tiempo_1 y tiempo_2 ha sido mayor de 900us, encender led de aviso.
  // Nunca hay que sobrepasar 1ms de tiempo en estado HIGH.
  tiempo_2 = micros();
  tiempo_ON = tiempo_2 - tiempo_1;
  if (tiempo_ON > 900) digitalWrite(pin_LED_rojo2, HIGH);   // Tiempo excedido
  // ------------------ ¡¡1ms max!! ------------------

Finalmente, sumamos estos tiempos ESC1_us, ESC2_us, ESC3_us y ESC4_us, que oscilarán entre 1000us y 2000us, con el instante en el que hemos puesto las señales en estado HIGH (tiempo_motores_start). De esta forma, obtenemos el instante en que hay que bajar las señales PWM a estado LOW para terminar el ciclo PWM. 

Utilizando el siguiente bucle while, pasamos a estado LOW  las cuatro señales PWM, cada una cuando corresponda. De esta forma conseguimos señales PWM de la misma frecuencia del ciclo que hayamos escogido:

while (digitalRead(pin_motor1) == HIGH || digitalRead(pin_motor2) == HIGH || digitalRead(pin_motor3) == HIGH || digitalRead(pin_motor4) == HIGH) {
 if (tiempo_motores_start + ESC1_us <= micros()) digitalWrite(pin_motor1, LOW);
 if (tiempo_motores_start + ESC2_us <= micros()) digitalWrite(pin_motor2, LOW);
 if (tiempo_motores_start + ESC3_us <= micros()) digitalWrite(pin_motor3, LOW);
 if (tiempo_motores_start + ESC4_us <= micros()) digitalWrite(pin_motor4, 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!

La programación de los controladores PID es también bastante fácil de entender: calculamos el error como hemos hecho más arriba, y lo multiplicamos por los valores de Kp, Ki y Kd. Intentad entender en código y la función de los tres parámetros del PID:

  PID_ang_Pitch_error = RC_Pitch_consigna - angulo_pitch;                        // Error entre lectura y consigna
  PID_ang_Pitch_P  = Pitch_ang_Kp  * PID_ang_Pitch_error;                        // Parte proporcional
  PID_ang_Pitch_I += (Pitch_ang_Ki * PID_ang_Pitch_error);                       // Parte integral (sumatorio del error en el tiempo)
  PID_ang_Pitch_I  = constrain(PID_ang_Pitch_I, -PID_ang_sat1, PID_ang_sat1);    // Limitar parte integral
  PID_ang_Pitch_D  = Pitch_ang_Kd * (angulo_pitch - angulo_pitch_ant);           // Parte derivativa (diferencia entre el error actual y el anterior)

  PID_ang_Pitch_OUT =  PID_ang_Pitch_P + PID_ang_Pitch_I + PID_ang_Pitch_D;      // Salida PID
  PID_ang_Pitch_OUT = constrain(PID_ang_Pitch_OUT, -PID_ang_sat2, PID_ang_sat2); // Limitar salida del PID

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 al no poder calcular su ángulo solo con el sensor MPU6050, sería necesario integrar un magnetómetro). 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 inclinación, 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 (º): 

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