Guía Montaje Drone) · 1. Conceptos generales sobre Drones Para el montaje de nuestro Dron, vamos a necesitar conocer cómo funciona un Dron, porqué gira a un lado u otro, sobre

Contenido 1. Conceptos generales sobre Drones ....................................................................................... 2

2. Montaje del Dron .................................................................................................................. 4

2.1 Material Esencial ........................................................................................................... 4

2.2 Otros componentes ....................................................................................................... 8

2.3 Pautas y Consejos .......................................................................................................... 9

2.4 Montaje de la estructura del Dron .............................................................................. 11

2.5 Montaje Hardware ...................................................................................................... 13

3. Programando partes del Dron ............................................................................................. 23

3.1 Mando radio control (RC) ............................................................................................ 23

3.2 Conexionado mando-Arduino ..................................................................................... 24

3.3 Código Arduino (Sketch) .............................................................................................. 26

3.4 El sensor MPU6050 ..................................................................................................... 32

3.5 Código Arduino para lectura del sensor MPU6050 ..................................................... 32

3.6 Estrategias de control de vuelo para drones .............................................................. 47

3.6.1 Modo de control ‘Acrobático’ ............................................................................. 47

3.6.2 Modo de control ‘Estable’ ................................................................................... 61

3.7 Código Arduino para control de motores Brushless con ESC ...................................... 64

3.8 Vibraciones de Motor, estudio y su eliminación. ........................................................ 69

3.8.1 Calibración de hélices y motores ........................................................................ 70

3.8.2 Código Arduino (Sketch) ...................................................................................... 73

3.8.3 Montaje de las hélices ......................................................................................... 78

3.8.4 Código para Arduino y Matlab. ........................................................................... 79

3.9 Resumen y Puesta en funcionamiento final del Dron. ................................................ 80

3.9.1 Comprobaciones ................................................................................................. 82

1. Conceptos generales sobre Drones

Para el montaje de nuestro Dron, vamos a necesitar conocer cómo funciona un Dron, porqué

gira a un lado u otro, sobre su propio eje, porqué avanza o retrocede… Todos estos movimientos,

que ya veremos más adelante con más atención, se llevan a cabo sobre diferentes ejes tal y

como se muestra en la imagen (Fig. 1):

Fig. 1

Gracias a estos tres ejes, junto al giro de formas precisas de las cuatro hélices del Dron,

podremos hacer que se mueva hacia delante o atrás, a los lados o gire sobre sí mismo. El

conocimiento del funcionamiento es esencial a la hora del montaje, ya que de ello, dependerá

el buen funcionamiento o no del Dron.

Fig. 2

En la Fig. 2, se muestran todos los movimientos esenciales que nos debe permitir el Dron llevar

a cabo para poder controlarlo perfectamente y cada uno de estos movimientos recibe un

nombre concreto con los que debemos familiarizarnos para hablar con propiedad y saber

exactamente a qué nos referimos en cada momento. En la imagen se muestra la posición del

Dron, el movimiento que se realiza y además, las hélices que se necesita que giren y además,

conocer el sentido en el que lo hacen, cosa muy importante a la hora de tener en cuenta el

montaje.

Climb/Descend (Elevación/Descenso): Pertenece a la primera imagen que podemos ver

en la Fig. 2, podemos observar que, dos de las hélices giran en sentido horario y otras

dos en sentido anti-horario, y que éstas, a su vez están enfrentadas entre sí, es decir, las

hélices pares girarán en sentido horario mientras que, las hélices impares, lo harán en

sentido anti-horario. Esta regla la deberemos seguir a lo largo de todo el montaje para

no cometer errores.

Roll (Alabeo): Puede ser un Roll/Alabeo, a Izquierda o derecha, en la imagen se muestra

un Roll a la derecha. Como se puede observar, en la imagen únicamente funcionan las

hélices número 2 y 3, es decir, funcionará una hélice con movimiento horario (hélice 2)

y la otra anti-horario (hélice 3), de esta forma, haremos que el dron se eleve un poco

por el lado donde están funcionando las hélices y por lo tanto, haga que el dron alabee

hacia la derecha. En el caso del alabeo hacia la izquierda, funcionarán las otras dos

hélices, es decir, hélice 1 y 4.

Pitch (Cabeceo): El pitch/Cabeceo, pude ser up/down, (Arriba/Abajo). El principio de

funcionamiento es exactamente igual que el explicado anteriormente con el Roll, lo

único que para que se lleve a cabo el cabeceo, necesitaremos que sean las hélices 3 y 4,

para hacer un Pitch-down o, al contrario, las hélices 1 y 2, para hacer un pitch-up.

Yaw (Guiñada): el Yaw/Guiñada, es el giro del dron sobre el eje vertical, es decir, sobre

el eje Z, que giraremos hacia la izquierda o hacia la derecha. En esta ocasión las hélices

que deben moverse, en función del giro que se quiera realizar (a izquierdas o a

derechas), deben ser las hélices contra puestas entre sí, siendo siempre las dos hélices

pares o las dos impares en función de la dirección del giro a realizar.

La estructura del Frame que vamos a utilizar es en forma de “X” (Fig. 3), aunque, también

podríamos haber optado por hacerlo en forma de “+”. Para una opción u otra, simplemente

habrá que tener en cuenta la posición del giróscopo y orientarlo siempre cara al que será el

frente de la dirección del Dron.

Fig. 3

Forma de “x” Forma de “+”

2. Montaje del Dron

Vamos a comenzar por conocer los principales componentes que vamos a necesitar con una

breve descripción de cada uno de ellos. Debemos revisar que tenemos todos los elementos

esenciales.

2.1 Material Esencial

1x MPU6050. El sensor más importante de nuestro dron, sin el cual sería imposible hacerlo volar

de forma controlada. Es el giróscopo que nos controlará de los diferentes giros y posiciones en

los que se encuentre nuestro Dron. Muy importante, cuando vayamos a hacer el montaje,

posicionar la flecha con la “Y” en el eje longitudinal de nuestro Dron.

Placa de montaje (para soldar componentes). Lo utilizaremos para soldar los diferentes

componentes del dron. Esta placa irá después situada encima del frame. (Se ha comprado en

principio por si fuese necesario, aunque, en lugar de ello vamos a hacer el montaje sobre

Protoboards para facilitar montajes y no malgastar mucho tiempo en soldaduras, además, si

cometemos un error soldando será más complejo subsanarlo que si se hace en Protoboard).

1x Mando y receptor RC. Lo utilizaremos para controlar el dron y hacer que se desplace a

nuestro antojo. Viene incluido el mando emisor y un receptor que debemos colocar en nuestro

Dron para recibir las señales que enviemos.

1x Batería 3S 22000mAh. Fuente de energía del dron. Utilizaremos una batería 3S de 2200mAh,

la cual nos proporcionará aproximadamente unos 20 minutos de vuelo sin parar, lo que, para

ser un Dron, es una duración fantástica.

1x Cargador de baterías Imax B6. Lo utilizaremos para recargar la batería.

1x Placa de distribución de potencia. Utilizaremos esta placa para soldar los cuatros ESC a la

batería de forma robusta. Esta conexión puede hacerse simplemente utilizando cable, pero es

más cómodo y seguro hacerlo utilizando una placa como esta.

1x LCD 16×2. Lo utilizaremos para visualizar algunas variables y alarmas que puedan surgir

durante el vuelo. Junto al LCD también debe haber una placa I2C para facilitar el conexionado

con el resto del circuito. Después veremos cómo se deben colocar el I2C al LCD.

1x Pack de leds. Los utilizaremos para visualizar parámetros durante el vuelo.

Conectores de bala (para los motores). Es imprescindible conectar los ESC y los motores de

forma robusta. Para ello utilizaremos conectores tipo bala.

Conector tornillo 2 pines (para las señales de control). Para conectar las salidas de la placa

Arduino con los diferentes periféricos (MPU6050, ESC, leds…) utilizaremos conectores

atornillados. Nos podrían ser útiles para conectar diferentes cables si utilizamos las placas de

soldaduras anteriores, pero como el montaje se va a realizar en Protoboards, en principio no

vamos a darle uso, no obstante, por si fuesen necesarias, están disponibles.

Crimpadora y terminales. Los utilizaremos para asegurar los cables de señal y facilitar su

conexionado al introducirlos en los terminales a tornillo:

Interruptor de 20Amp. Lo utilizaremos para apagar y encender el dron (conectar/desconectar

la batería) de forma segura y controlada.

2.2 Otros componentes

Estañador (y estaño). Imprescindible para soldar los diferentes componentes de forma robusta.

Polímetro. Medidor de tensiones y corrientes (Voltaje e Intensidades) del circuito, probar

conductividad eléctrica o probar soldaduras correctas.

2.3 Pautas y Consejos

Algunos consejos a tener en cuenta antes de comenzar con el montaje del Dron que nos ayuden

y facilite la tarea y además nos ayuden a que el funcionamiento final sea mucho mejor y

optimizado.

Os dejo una serie de consejos y pautas generales de montaje para facilitar la tarea. Si tenéis

cualquier duda no dudéis en preguntármelo en los comentarios, intentaré resolver vuestras

dudas lo antes posible:

Intentad que el peso esté equilibrado y lo más centrado posible, colocad la batería lo más baja

y centrada posible. Agarradla bien utilizando alguna brida. La situamos en la parte baja del

frame, y utilizamos la ‘placa de distribución de potencia’ para conectar los cuatro ESC a la tensión

de batería.

El sensor siempre tiene que estar orientado con uno de los ejes de movimiento. Bastará con

orientar la ‘fecha Y’ del sensor con lo que consideremos la ‘parte delantera’ de nuestro dron, de

esta forma, alinearemos este eje con nuestro eje Pitch. Ahora, cuando movamos hacia arriba el

stick del eje Pitch, el dron se moverá hacia ‘delante’, es decir, hacia la dirección que indica la

‘flecha Y’ (Fig. 4).

Recordad que vamos a construir un dron tipo ‘x‘, por lo que no deberemos orientar el sensor

con ninguno de los brazos del frame tal y como se mencionó anteriormente (Si hay dudas, volver

atrás y revisar tipo de montaje en “x” o ”+”).

Fig. 4

Enumerar las hélices y seguir el esquema de color para pares e impares. Para el correcto

funcionamiento del montaje, ya hemos hablado anteriormente de la importancia de saber qué

motor debe girar en sentido horario o anti-horario, para ello y para que tengamos un patrón

general para todos los alumnos, se van a establecer las siguientes normas:

Como el Frame del Dron viene en color blanco y rojo, vamos a utilizar siempre:

Los brazos de color Blanco, para los motores o hélices Impares, es decir la posición 1 y la 3.

Los brazos de color Rojo, para los motores o hélices pares, es decir la posición 2 y 4.

Enumeramos los brazos de la siguiente forma:

Siendo el motor número 1, el que está

posicionado delante a la derecha.

El número 2, será el motor delantero

izquierdo.

El número 3, el motor izquierdo trasero.

Por último, el número 4, será el motor

trasero derecho.

Que dando con muestra la fig. 5.

Fig. 5

Por último, como advertencia, NUNCA SE PONDRÁN LAS HELICES EN LOS MOTORES. Como

medida de precaución, NO está permitido poner las hélices a los Drones hasta que Agustín o

Francisco lo hayan indicado y hayan dado el visto bueno. De no haber alguna conexión bien

hecha, o de haber cualquier problema, las hélices podrían empezar a girar y ocasionar daños

muy graves. De no cumplir esta norma, será culpable el alumno que haya colocado las hélices

sin consentimiento.

2.4 Montaje de la estructura del Dron

Para el montaje del dron, se va a dividir en dos partes diferentes, por un lado, estará la

construcción de la estructura, la cual soportará los diferentes componentes que harán volar el

Dron. A esta estructura se le conoce con el nombre de Frame. En el kit que se os entrega, podréis

encontrar las diferentes partes de que se compone:

Los soportes para los motores y hélices, la estructura metálica central (el cuerpo del dron) en la

que iremos conectado las placas, sensores, baterías etc. Y que a su vez, se atornillarán a los

brazos soportes de los motores. Por último, encontraréis las patas blancas para que el Dron

tenga mayor altura cuando esté posado en el suelo.

Como recomendación, aconsejamos no montar las patas del Dron hasta haber finalizado por

completo todo el montaje electrónico y de hardware, ya que tendremos que estar girando y

moviendo de forma constante el Dron hasta haber terminado su montaje por completo.

El montaje de la estructura es muy simple aunque algo laborioso por el montón de tornillos que

lleva y porque utiliza un cabezal tipo Allen.

Comenzamos posicionando las patas, en el orden indicado anteriormente, muy importante, para

que esté generalizado y evitar posibles problemas posteriores. (Fig. 6)

Fig. 6

A continuación, unimos los brazos con la estructura metálica central. Hay dos, una más pequeña

que la otra, colocamos la placa metálica más pequeña sobre los brazos y la más grande por la

parte de debajo de los brazos, tal como se muestra a continuación.

Es muy simple, ya que los orificios de los brazos y de las placas metálicas coincidirán

perfectamente. Únicamente tener en cuenta que las caras de las placas metálicas queden tal y

como se ve en la imagen superior, ya que la placa grande tiene una cara para poder hacerle

soldaduras, mientras que la otra es casi en su totalidad negra. Intentamos dejar hacia abajo la

cara de la placa superior por si necesitamos soldar algo, podamos hacerlo sin dificultad.

Una vez tenemos ya el Frame montado y veamos que queda bastante robusto, es hora de añadir

los motores en los brazos. Para ello, hay cuatro kits por grupo, donde cada uno de ellos incluye

dos hélices, un motor, tornillos y una placa metálica. (Fig. 7).

Fig. 7

Para que el motor quede bien sujeto, vamos a colocar la chapa metálica por debajo del brazo

que sujeta al motor, y por la parte superior, colocamos el motor y apretamos los tornillos como

se ve en la imagen. Fig. 8. Muy importante, la chapa sólo tiene una posición correcta para que

los orificios de la placa metálica coincidan con los orificios que sujetarán el motor en el brazo

soporte. Debemos asegurarnos antes de apretar el primer tornillo, que todos los orificios

corresponden correctamente con los del motor y con los del brazo.

Fig. 8

Dejamos sin conectar por ahora el ESC (elemento amarillo con cables azules que va incluido en

el kit de cada uno de los motores), ya que esa parte se llevará a cabo más adelante en el montaje

de Hardware del Dron y habrá que tener en cuenta varias cosas para su correcta conexión.

2.5 Montaje Hardware

Una vez terminada la parte del montaje estructural del Dron, tenemos el cuerpo y la superficie

para poder ir conectando y agregando todas las partes hardware que requiere para controlar y

volar el Dron de forma controlada y correcta.

Para continuar, vamos a hacerlo terminando las conexiones de los motores con los ESC. Este

paso es una parte muy muy importante de la construcción de un Dron, ya que de la correcta

instalación y conexión de estos elementos dependerá que el Dron haga lo que necesitamos que

haga o lo que requiramos que haga. Como se ha explicado anteriormente (Fig. 2), dependiendo

de si vamos a realizar un Roll, Pitch, Yaw o necesitemos dar más potencia o menos (Throttle), o

aumentar o disminuir altitud, las hélices deben girar en un sentido u otro y además, deberán

girar en cada ocasión unas u otras. Aquí es donde cobra importancia que las hélices deban girar

en un sentido en las pares e impares y para que el giro sea inverso o no se hará con la conexión

que vamos a hacer a continuación del motor con el ESC.

Los motores en lugares pares, girarán en sentido horario, mientras que los impares al cambiar

la conexión como se muestra en el diagrama, lo harán en sentido anti-horario.

Para el resto de conexiones y elementos que necesitamos conectar vamos a empezar a utilizar

la Protoboard y tendremos que seguir el esquema electrónico que se muestra a continuación en

la Fig. 9.

Colocamos el Arduino, el MP9050 y la placa de Distribución como se muestra en la imagen para

asegurar la placa distribuidora, usaremos unas bridas de sujeción.

A continuación, conectamos los GND y las VCC como se muestra a continuación.

3. Programando partes del Dron

3.1 Mando radio control (RC)

Una de las maneras más fáciles y baratas de comunicarse con nuestro futuro drone es utilizar

un mando RC y un receptor. Son fáciles de utilizar. Como todo en la electrónica, el mundo de los

mandos RC es muy amplio pero para nuestro dron es suficiente. Dispondremos de un canal para

controlar el throttle (Potencia), otro para el pitch (Cabeceo), uno más para el rol (Alabeol) y el último

para el yaw (Guiñada). Se ha decidido asociar los sticks con las siguientes funciones o movimientos

del drone, ya que hay muchas otras configuraciones que se pueden llevar a cabo y podremos encontrar

en otros controles remotos de Drones.

Se ha elegido esta configuración ya que, con el Stick izq, vamos a controlar todos los valores que

dependen del eje Z (Throttle y Yaw), mientras que con el Stick derecho, vamos a poder controlar el

eje x (Roll) y el eje y (Pitch). Además esta es la forma más extendida por lo que nos facilitará las

cosas.

El funcionamiento de un mando RC es muy simple. El movimiento de los sticks del mando es

procesado y enviado por ondas de radio a nuestro receptor, que irá situado en el frame del

drone. Esta información se compone de señales PWM de 50Hz que varían en función de la posición

de cada stick. Si accionamos al mínimo alguna de las palancas del mando, el ancho de pulso que

recibamos en el receptor será de 1ms, y si la accionamos al máximo, el ancho del pulso será de 2ms

(estos valores pueden ser diferentes en cada mando). Si dejamos la palanca en el punto central

obtendremos pulsos de 1.5ms aproximadamente, siempre manteniendo una frecuencia de 50Hz. Esta

información será enviada a la placa Arduino y se utilizara como referencia para mover el drone.

Como vemos en la imagen anterior, el mando no da exactamente pulsos de 1ms con el stick al mínimo,

ni pulsos de exactamente 2ms subiéndolo a máximo. Estos pequeños errores de precisión del mando

los corregiremos más adelante al final.

3.2 Conexionado mando-Arduino

El receptor ira situado en el frame y es necesario alimentarlo y cablearlo de forma adecuada.

Alimentaremos el receptor a +5V en cualquiera de los canales centrales, por ejemplo con la

tensión de nuestra placa Arduino. Si hemos configurado bien la unión mando-receptor, al encender el

mando debería encenderse un led rojo (en mi caso) que indica que funciona correctamente.

El conexionado necesario para ejecutar el código que os dejo al final de este artículo se muestra a

continuación. Simplemente hay que alimentar el receptor en cualquiera de los canales centrales y

cablear las cuatro señales a las correspondientes entrada digitales, en mi caso:

Pitch ⇒ CH2 ⇒ IN 12

Throttle ⇒ CH3 ⇒ IN 8

Roll ⇒ CH1 ⇒ IN 9

Yaw ⇒ CH4 ⇒ IN 7

Antes de continuar me gustaría analizar un poco más en detalle el funcionamiento de mi receptor y la

forma en la que genera los pulsos. La siguiente imagen muestra una captura sacada con osciloscopio

donde visualizo las señales de pitch, roll y yaw (no he metido el canal de throttle por que mi

osciloscopio solo tiene 3 canales). De esta imagen he sacado los siguientes datos:

El receptor genera pulsos de 3.3V.

Los pulsos se generan de forma secuencial, hasta que no termina un pulso no empieza el siguiente.

Nunca habrá más de un canal en estado HIGH cada vez.

3.3 Código Arduino (Sketch)

La forma más sencilla de leer este tipo de pulsos es utilizar la función PulseIn de Arduino.

Lamentablemente esta función tan fácil de utilizar no sirve para esta aplicación (aunque sí para muchas

otras). Pero, ¿Por qué? Como todos a estas alturas sabemos, el código de Arduino se ejecuta de manera

secuencial, línea tras línea y de arriba abajo. Arduino no ejecuta una línea si no ha terminado de

ejecutar la anterior. Cuando llegue a la línea PulseIn(), Arduino se quedará esperando en ella el tiempo

que haga falta hasta recibir un pulso y medirlo. Como sabemos, el mando solo nos enviará un pulso

por canal cada 20ms, por lo que esta función PulseIn estará esperando a recibir el pulso (y sin hacer

nada mas) durante 20ms. Teniendo en cuenta que contamos con 4 canales, la demora se puede

prolongar hasta 80ms. Este intervalo de tiempo que a priori puede parecer muy corto, para nuestros

drone es toda una eternidad y hará que nunca sea estable (esto lo veremos en otro apartado más

adelante).

Localización del receptor en el frame

¿Cómo podemos leer las señales recibidas desde el mando de una forma eficiente? Utilizando

interrupciones hardware, una por canal. El funcionamiento de una interrupción hardware es muy

simple: ante un flanco positivo (cambio de estado de LOW a HIGH) o negativo (cambio de estado

de HIGH a LOW) en alguna de las interrupciones (pins), Arduino detendrá la ejecución normal

del programa y ejecutara la parte del código que hayamos asociado a esa interrupción en

concreto. Una vez ejecutado esta parte de código, Arduino regresará al programa principal y seguirá

ejecutándolo donde lo dejó. A continuación os explicaré como leer los canales de vuestro mando RC

de forma que la lectura solo lleve unos poco microsegundos (100 veces menos que con la función

PulseIn). Para poder poner esto en marcha necesitamos 4 interrupciones hardware, una por cada

canal que queremos leer. La paca Arduino Nano cuenta únicamente con dos interrupciones

hardware, pero existen librerías como EnableInterrupt.h que permiten convertir casi cualquier pin

analógico o digital en interrupción. Esta librería es extremadamente simple de usar como veremos más

adelante.

Al final de este artículo podréis encontrar el sketch completo que utilizo para medir los pulsos con

interrupciones hardware. El sketch es muy simple como veréis a continuación. Cuando se detecta

un pulso (flanco positivo o negativo) en alguna de las cuatro interrupciones, se ejecuta la parte del

código asociado a esa interrupción. El primer paso es identificar si hemos detectado un flanco positivo

o uno negativo, para lo que bastará con leer el estado de la propia entrada digital. Si está en estado

HIGH significará que hemos detectado un flanco positivo (comienzo del pulso) y si está en LOW, uno

negativo (fin del pulso). Cuando se detecta un flanco positivo se registra el tiempo (el instante) en

el que se ha dado la interrupción, para lo que utilizaremos la función micros() y lo guardamos en una

variable. Una vez hecho esto Arduino sale de la interrupción y sigue ejecutando el código principal.

Un tiempo después se detecta un flanco negativo (cambio de estado de HIGH a LOW), por lo que se

vuelve a ejecutar la interrupción. Volvemos a leer el estado de la entrada que esta vez estará en estado

LOW. Finalmente detenemos el correspondiente contador y calculamos el tiempo que ha

transcurrido desde que lo hemos activado, que será el tiempo que ha durado el pulso. A

continuación os dejo el código completo para que podáis leer los canales de vuestro mando:

La diferencia de mi sketch con la función PulseIn es que mi programa no está esperando (y sin hacer

nada mas) hasta recibir un pulso para seguir ejecutando el loop principal. El programa solo deja de

ejecutarse mediante interrupciones para poner en marcha un contador y después detenerlo.

Todo el tiempo restante continúa funcionando y ejecutando el control principal. Gracias a esto

conseguimos leer los 4 canales del mando RC prácticamente sin consumir nada de tiempo. Os dejo el

código completo para que podáis leer los canales de vuestro mando. El conexionado necesario es el

mismo que hemos visto mas arriba:

Roll ⇒ IN 9

Pitch ⇒ IN 12

Yaw ⇒ IN 7

Throttle ⇒ IN 8

Para poder seguir es importante que hayas conseguido poner en marcha esta parte de forma

adecuada (programa con el nombre MaxMinArduino). Os tenéis que asegurar que cada canal del

mando corresponde con los movimientos del drone que hemos asignado. Moved la palanca asociada

al Pitch y comprobad que es la variable PulsoPitch la que cambia y haced lo mismo con los demás

canales. En el caso de que no estén bien asignados, podéis modificar el nombre de las variables

(recomendado) o modificar el cableado, pero asegurad que moviendo el stick asociado al movimiento

Pitch es la variable PulsoPitch la que cambia y no ninguna otra.

Los canales del mando están configurado de la siguiente manera. Comprobad como están

configurados y de que lo esté correctamente. En caso de que alguno este invertido respecto a lo que se

propone a continuación, habrá que ajustarlo más adelante. En algunos casos, el canal de throttle está

invertido de fábrica (es cuestión de cómo esté diseñado el mando que compremos), ya que al hacer

este ejemplo se leían 2000us con el stick al mínimo y 1000us con el stick al máximo:

Stick de Pitch arriba: 1000us aprox

Stick de Pitch abajo: 2000us aprox

Stick de Roll derecha: 2000us aprox

Stick de Roll izquierda: 1000us aprox

Stick de Yaw derecha: 1000us aprox

Stick de Yaw izquierda: 2000us aprox

Stick throttle arriba: 1000us aprox

Stick throttle abajo: 2000us aprox

Como hemos visto más arriba, el mando no da exactamente pulsos de 1ms con el stick al mínimo, ni

pulsos de exactamente 2ms subiéndolo a máximo. Estos pequeños errores de precisión del mando han

de ser corregidos, especialmente para el canal destinado al throttle. Para ello utilizaremos en siguiente

código (con el nombre mandoCompletoINT), que simplemente lee los cuatro canales y visualiza los

valores máximo y mínimos mientras movemos los stick de un lado a otro. Anotaremos la duración de

los cuatro pulsos en los extremos de cada canal, moviendo los cuatro stick del mando arriba/abajo e

izquierda/derecha hasta conseguir los valores máximos y mínimos de pulso.

Estos valores después serán utilizados para escalar las lecturas del mando a nuestras necesidades para

utilizarlos como consigna (referencia) para los controladores PID.

Descargar los archivos ‘mandoCompletoINT‘ y ‘MaxMinArduino‘.

Estos números después se utilizarán para escalar el throttle. Para el Pitch, Roll y Yaw asumo que el

mando es ideal y los pulsos varían exactamente entre 1ms y 2ms, ya que son menos críticos. Es

importante que en el código que os dejo más adelante para poner en marcha y controlar los

motores modifiquéis los siguientes parámetros en función de lo que haya arrojado el software de

arriba:

// MANDO POTENCIA

const int PulsoMaxPotencia = 2000;

const int PulsoMinPotencia = 980;

const float tMaxPotencia = 1.72; // En esta línea

const float tMinPotencia = 1.06; // y En esta línea

Ahora que ya sabemos cómo leer pulsos con nuestra placa Arduino, ¿qué hacemos con esta señal?

¿Cómo le decimos a nuestro drone que avance o que gire en una dirección determinada mediante un

pulso en microsegundos?

Como ya sabemos, para poder girar o avanzar, el drone tiene que ser capaz de inclinarse

manteniéndose estable en el aire. Para ello, tenemos que indicar al drone cuantos grados queremos

que se incline en una dirección determinada para poder desplazarse hacia donde se le ha

indicado. Por lo tanto, habrá que usar estas señales del mando RC como referencia para inclinar el

drone en un eje o en otro. Al fin y al cabo, esa es la función final del mando: indicar al drone cuantos

grados queremos que se incline en un eje determinado para poder desplazarnos en una dirección

concreta. Para ello, es necesario ‘procesar’ la información en microsegundos que nos llega del

receptor y convertirla a una señal proporcional en grado de inclinación.

Pongamos como ejemplo el stick asociado al eje Pitch. Tenemos que procesar la señal que recibimos

del receptor (variable PulsoPitch), de forma que con el stick es su posición central (sin moverlo ni

arriba ni abajo), el drone reciba una consigna de 0º de inclinación, es decir, que se mantenga estable

y sin inclinarse.

PulsoPitch = 1500us (stick en posición central) ⇒ 0º de inclinación

Que, si por el contrario movemos el stick hasta arriba, el drone reciba la orden de inclinarse unos -30º

en ese eje para desplazarse en esa dirección.

PulsoPitch = 1000us (stick arriba) ⇒ -30º de inclinación

Y que, si movemos el stick hacia abajo, el drone reciba la orden de inclinarse unos +30º en ese eje

para desplazarse en dirección contraria.

PulsoPitch = 2000us (stick abajo) ⇒ +30º de inclinación

Le estamos indicando al drone que se incline en el eje Pitch en un sentido o en otro (-30º/+30º)

en función de la posición del stick del mando RC.

Para procesar las señales obtenidas del mando y transformarlas en consigna de inclinación,

utilizaremos las siguientes ecuaciones. Hacen exactamente lo mismo que la función map() de Arduino.

El simplemente la ecuación de una recta que pasa por dos puntos:

El software lo podemos encontrar en el archivo llamado: “MandoCompletoProces”

Lo más importante es asegurarnos que una vez procesadas las señales, el canal de throttle no ha

quedado invertido. Es decir, con el stick al mínimo tenemos que obtener una salida de 1000us

aproximadamente, y con el stick al máximo una salida de 2000us aproximadamente. Si tenéis el canal

invertido, corregirlo cambiando de orden los parámetros de entrada.

De esto:

const float tMaxPotencia = 1.83; // <-- Si teneis la entrada Throttle invertida sustituid este valor

const float tMinPotencia = 1.12; // <-- por este y viceversa

A esto:

const float tMaxPotencia = 1.12; // <-- Si teneis la entrada Throttle invertida sustituid este valor

const float tMinPotencia = 1.83; // <-- por este y viceversa

Comprobar también los demás canales, y en caso de tener alguno invertido, seguid el mismo

procedimiento que con el canal throttle hasta ajustarlos como se muestra a continuación:

Moviendo stick de Pitch hacia arriba, wPitchConsigna = -30º aprox.

Moviendo stick de Pitch hacia abajo, wPitchConsigna = +30º aprox.

Moviendo stick de Roll hacia la derecha, wRollConsigna = +30º aprox.

Moviendo stick de Roll hacia la izquierda, wRollConsigna = -30º aprox.

Moviendo stick de Yaw hacia la derecha, wYawConsigna = +30º aprox.

Moviendo stick de Yaw hacia la izquierda, wYawConsigna = -30º aprox.

Las implicaciones de no hacer esta modificación en los canales Pitch, Roll y Yaw no son tan graves,

simplemente, cuando ordenáramos al drone ‘avanzar’, este retrocedería. Nada grave, pero conviene

corregirlo.

Acordaos de cambiarlo también cuando subamos el código final de nuestro Arduino que tendrá el

nombre de: “SoftwarePrincipal_v1.01_arduino1.8.5”

3.4 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.

Sensor MPU6050

3.5 Código Arduino para lectura del sensor MPU6050

Más adelante os dejo el código que vamos a utilizar 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, más adelante en este documento

iremos explicándolos más en detalle:

Descargamos el programa con nombre: “IMUangle”

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 más o menos la salida:

Una vez configurado el sensor basta con ir haciendo lecturas periódicas, para lo que utilizaremos

las siguientes líneas de código. Simplemente avisamos al sensor de que necesitamos una nueva

lectura, y este nos contesta con toda la información:

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

//================= Leer MPU6050

void MPU_6050() {

Wire.beginTransmission(MPU6050_address); // Empezamos comunicación

Wire.write(0x3B);

Wire.endTransmission();

Wire.requestFrom(MPU6050_address, 14); // Pedimos 14 bytes

while (Wire.available() < 14); // Espramos hasta recibir las informacion

ax = Wire.read() << 8 | Wire.read();

ay = Wire.read() << 8 | Wire.read();

az = Wire.read() << 8 | Wire.read();

temperature = Wire.read() << 8 | Wire.read();

gx = Wire.read() << 8 | Wire.read();

gy = Wire.read() << 8 | Wire.read();

gz = Wire.read() << 8 | Wire.read();

}

A continuación os dejo el código de lectura de valores raw (Archivo con nombre:

“IMUraw”) (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**:

Conexionado MPU6050

** 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.

Tuvimos por algún tiempo un problema que tardamos en solucionar bastante, 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 ha sido siempre

0x68, pero puede pasar que la dirección de vuestro sensor sea diferente. Para saber

exactamente cuál es la dirección podéis utilizar el siguiente código, simplemente cableáis y

alimentáis el sensor como hemos visto más 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.

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 cómo entender y procesar toda esta información:

1

2

3

4

#include <Wire.h>

// MPU6050

#define MPU6050_address 0x68

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

int gx, gy, gz, loop_counter;

float ax, ay, az, temperature, loop_timer;

//=============================================//////////////////// SETUP

////////////////////=============================================

void setup() {

Wire.begin();

init_gyro(); // Inicializar MPU

Serial.begin(115200);

loop_timer = micros();

}

// =============================================//////////////////// LOOP

////////////////////=============================================

void loop() {

while (micros() - loop_timer < 3000);


MPU_6050(); // Leer MPU

if (loop_counter == 0)Serial.print(ax);

if (loop_counter == 1)Serial.print("\t");

if (loop_counter == 2)Serial.print(ay);


if (loop_counter == 4)Serial.print(az);


if (loop_counter == 6)Serial.print(gx);


if (loop_counter == 8)Serial.print(gy);

36

37

38

39

40

41

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

66


if (loop_counter == 10)Serial.println(gz);

loop_counter ++;

if (loop_counter == 11)loop_counter = 0;

}

//================= Leer MPU6050

void MPU_6050() {


Wire.write(0x3B);











}

//================= Subrutina inicilialización (solo se ejecuta una vez al iniciar el

programa)

void init_gyro() {

Wire.beginTransmission(MPU6050_address);

Wire.write(0x6B); // PWR_MGMT_1 registro 6B hex

Wire.write(0x00); // 00000000 para activar


67

68

69

70

71

72


Wire.write(0x1B); // GYRO_CONFIG registro 1B hex

Wire.write(0x08); // 00001000: 500dps



Wire.write(0x1C); // ACCEL_CONFIG registro 1C hex

Wire.write(0x10); // 00010000: +/- 8g


}

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 más

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 más 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 :

1

2

3

4

// ================================== Calculo w

pitchGyro = gx / 65.5;

rollGyro = gy / 65.5;

yawGyro = 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 han 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:

1

2

angulo_pitch += pitchGyro * tiempo_ejecucion / 1000 ;

angulo_roll += rollGyro * 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. Preguntar a Fran para dudas sobre el DRIFT o DERIVA.

Para 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, compensamos la desviación producida por el

efecto drift:

1

2

3

4

5

6

7

8

9

1

0

1

1

1

2

1

3

1

4

1

5

1

6

1

7

1

8

1

9

2

0

//================= Calculo de w y angulo

void ProcesMPU() {

//

========================================================================

========================== Calibracion

ax -= angle_pitch_acc_cal;

ay -= angle_roll_acc_cal;

az -= angle_yaw_acc_cal;

az = az + 4096;

//

========================================================================

========================== Calculo w

pitchGyro = gx / 65.5; // 65.5: si leo 65.5 en raw, significa que gira a 1º/s

rollGyro = gy / 65.5;

yawGyro = gz / 65.5;

//

========================================================================

========================== Calculo ang



angulo_pitch += angulo_roll * sin((gz - gyro_Z_cal) * tiempo_ejecucion *

0.000000266); // tiempo_ejecucion/1000 /65.5 * PI/180

angulo_roll -= angulo_pitch * sin((gz - gyro_Z_cal) * tiempo_ejecucion * 0.000000266);

acc_total_vector = sqrt(pow(ay, 2) + pow(ax, 2) + pow(az, 2));

angle_pitch_acc = asin((float)ay / acc_total_vector) * 57.2958; // 57.2958 = Conversion

de radianes a grados 180/PI

2

1

2

2

2

3

2

4

2

5

2

6

2

7

angle_roll_acc = asin((float)ax / acc_total_vector) * -57.2958;

angulo_pitch = angulo_pitch * 0.99 + angle_pitch_acc * 0.01; // Corregimos en drift

con filtro complementario

angulo_roll = angulo_roll * 0.99 + angle_roll_acc * 0.01;

}

** Cuidado aquí. 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 más adelante cuando hablemos 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 podemos hacer 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 más 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:

Nombre del archivo: “IMUangle”

1

2

3

4

5

6

7

// ===========================================

#define usCiclo 5000 // Ciclo de ejecucion de software en microsegundos

// ==========================================

#include <Wire.h>

// MPU6050

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36

37

38

#define MPU6050_address 0x68

float angulo_pitch, angulo_roll, yawGyro, pitchGyro, rollGyro, angle_pitch_acc,

angle_roll_acc, 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 angle_yaw_acc_cal, angle_roll_acc_cal, angle_pitch_acc_cal;

long tiempo_ejecucion, loop_timer;

//=============================================//////////////////// SETUP

////////////////////=============================================

void setup() {

Wire.begin();


pinMode(11, OUTPUT); // Led azul

init_gyro(); // Inicializar MPU

digitalWrite(11, HIGH); // Encendemos un Led durante la calibracion

//==== Calibrar giroscopio

for (cal_int = 0; cal_int < 3000 ; cal_int ++) {

MPU_6050(); // Leemos los datos del MPU6050 3000 veces y sacamos el valor

medio para obtener los offset del giroscopio

gyro_X_cal += gx;

gyro_Y_cal += gy;

gyro_Z_cal += gz;

delayMicroseconds(1000);

}

gyro_X_cal = gyro_X_cal / 3000; // valor medio de las 3000 muestras

gyro_Y_cal = gyro_Y_cal / 3000;

gyro_Z_cal = gyro_Z_cal / 3000;

39

40

41

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

66

67

68

69

//==== Calibrar acelerometro


MPU_6050();

angle_pitch_acc_cal += ax;

angle_roll_acc_cal += ay;

angle_yaw_acc_cal += az;

}

angle_pitch_acc_cal = angle_pitch_acc_cal / 3000;

angle_roll_acc_cal = angle_roll_acc_cal / 3000;

angle_yaw_acc_cal = angle_yaw_acc_cal / 3000;

accCalibOK = true;

digitalWrite(11, LOW);


}

// =============================================//////////////////// LOOP

////////////////////=============================================

void loop() {

while (micros() - loop_timer < usCiclo);

tiempo_ejecucion = (micros() - loop_timer) / 1000;


MPU_6050(); // Leer MPU

ProcesMPU(); // Procesar datos MPU

Serial.print(angulo_pitch); // Tenéis que abrir el Serial Plotter

Serial.print("\t");

Serial.println(angulo_roll);

}

70

71

72

73

74

75

76

77

78

79

80

81

82

83

84

85

86

87

88

89

90

91

92

93

94

95

96

97

98

99

//================= Leer MPU6050

void MPU_6050() {


Wire.write(0x3B);











}

//================= Calculo de w y angulo

void ProcesMPU() {

//

=======================================================================

=========================== Calibracion

ax -= angle_pitch_acc_cal;

ay -= angle_roll_acc_cal;

az -= angle_yaw_acc_cal;

az = az + 4096;

//

=======================================================================

=========================== Calculo w

10

0

10

1

10

2

10

3

10

4

10

5

10

6

10

7

10

8

10

9

11

0

11

1

11

2

11

3

11

4

11

5

11

6

11

7

pitchGyro = (gx - gyro_X_cal) / 65.5; // 65.5: si leo 65.5 en raw, significa que gira a 1º/s

rollGyro = (gy - gyro_Y_cal) / 65.5;

yawGyro = (gz - gyro_Z_cal) / 65.5;

//

=======================================================================

=========================== Calculo ang



angulo_pitch += angulo_roll * sin((gz - gyro_Z_cal) * tiempo_ejecucion *

0.000000266); // tiempo_ejecucion/1000 /65.5 * PI/180

angulo_roll -= angulo_pitch * sin((gz - gyro_Z_cal) * tiempo_ejecucion * 0.000000266);

acc_total_vector = sqrt(pow(ay, 2) + pow(ax, 2) + pow(az, 2));

angle_pitch_acc = asin((float)ay / acc_total_vector) * 57.2958; // 57.2958 =

Conversion de radianes a grados 180/PI

angle_roll_acc = asin((float)ax / acc_total_vector) * -57.2958;

angulo_pitch = angulo_pitch * 0.99 + angle_pitch_acc * 0.01; // Corregimos en drift

con filtro complementario


}

//================= Subrutina inicilialización (solo se ejecuta una vez al iniciar el

programa)

void init_gyro() {


Wire.write(0x6B); // PWR_MGMT_1 registro 6B hex

Wire.write(0x00); // 00000000 para activar



Wire.write(0x1B); // GYRO_CONFIG registro 1B hex

Wire.write(0x08); // 00001000: 500dps

11

8

11

9

12

0

12

1

12

2

12

3

12

4

12

5

12

6

12

7

12

8

12

9

13

0

13

1

13

2



Wire.write(0x1C); // ACCEL_CONFIG registro 1C hex

Wire.write(0x10); // 00010000: +/- 8g



Wire.write(0x1A); // LPF registro 1A hex

Wire.write(0x03); // 256Hz(0ms):0x00 - 188Hz(2ms):0x01 -

98Hz(3ms):0x02 - 42Hz(4.9ms):0x03 - 20Hz(8.5ms):0x04 - 10Hz(13.8ms):0x05 -

5Hz(19ms):0x06


}

3.6 Estrategias de control de vuelo para drones

Las estrategias de control más comunes utilizadas en el mundo de los drones son el modo

acrobático y el modo estable. En este apartado, vamos a entender el funcionamiento de las dos

estrategias.

3.6.1 Modo de control ‘Acrobático’

En este primer modo de control solo utilizaremos las lecturas de velocidad angular calculadas a

partir de los datos obtenidos del sensor MPU6050. La velocidad la vamos a medir en ‘º/s’, es

decir, cuantos grados rota cada eje por segundo. Si por ejemplo uno de nuestros ejes da una

vuelta completa en un segundo, la velocidad será de 360º/s.

Vamos a empezar repasando unos conceptos básicos de funcionamiento. Si el eje pitch de

nuestro drone rotara por cualquier razón, porque uno de los motores tiene más potencia,

porque hay viento… el control tendrá que contrarrestar esta desviación actuando sobre los

motores correspondientes. En este caso, habría que actuar sobre los motores de la izquierda

acelerándolos, y sobre los motores de la derecha decelerándolos. De esta forma conseguiríamos

contrarrestar el efecto de la perturbación que ha hecho que nuestro drone rotara en su eje pitch.

Pero cuidado, en modo acrobático en drone no volverá a su posición inicial, simplemente

compensaremos la rotación hasta detener el drone. Esto es debido a que únicamente estamos

utilizando como referencia la velocidad de rotación de los ejes:

En modo acrobático simplemente contrarrestamos la rotación

Ahora bien, ¿cuánta potencia y durante cuánto tiempo hay que acelerar cada motor para

contrarrestar estas perturbaciones? En otras palabras, ¿Cómo hacemos que nuestro control sea

capaz de mantener el drone estático en el aire de forma autónoma? Para esto tenemos los PID.

Empecemos por lo básico. Si representáramos el algoritmo de control de estabilidad mediante

una conversación entre los diferentes componentes del drone, obtendríamos algo como esto:

Humano (mediante el mando RC): Ey drone, quiero que te mantengas estático en el aire, las

velocidades angulares de tus tres ejes a 0º/s (sin ninguna rotación en ningún sentido).

Drone (Arduino): recibido humano. MPU6050, necesito que me digas a qué velocidad nos

movemos en los tres ángulos.

Drone (MPU6050): Según mis mediciones rotamos a 0º/s de pitch, 0º/s de yaw y -5º/s de roll.

Drone (Arduino): Recibido MPU6050, parece que tenemos una desviación de 5º/s en el roll

respecto a lo que nuestro humano nos ha pedido. Atentos motores, tenemos que corregir la

desviación en la velocidad del eje roll. Acelera el motor 3 y frena el 1 → PIDS

Drone (Motores): recibido Arduino. Aplicando cambios en la consigna a los motores.

Y vuelta al punto 1)

Dicho de una manera más ‘técnica’ la secuencia quedaría de la siguiente manera. Como hemos

visto en la parte dedicada a la lectura del mando RC, vamos a utilizar interrupciones hardware

para la función de lectura RC (si no has leído la parte del mando RC, te aconsejo que lo hagas

antes de continuar para poder entender mejor el resto). El loop se ejecutará periódicamente y

cada vez que haya una interrupción hardware se leerán los ángulos que transmite el mando RC.

Secuencia: Esta secuencia se ejecuta cada 6ms

Arduino pide lecturas al sensor MPU6050 para calcular las velocidades de rotación en los ejes

roll, pitch y yaw.

El sensor MPU6050 responde con sus lecturas y se realizan los cálculos de velocidad angular en

º/s.

Con estas lecturas y las consignas que recibimos desde el mando, los PID calculan el error y

cuanto acelerar o decelerar cada motor para compensarlo.

Vuelta al paso 1)

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

A este proceso se le llama ‘Lazo de control’. Es muy importante tener en mente que este lazo de

control se ejecutará una y otra vez cada 6ms.

Diagrama de bloques de un controlador PID

Como vemos, los encargados de calcular el error de cada ángulo y actuar en consecuencia son

los PID. Los PID son una parte fundamental de nuestro drone por lo que es necesario saber algo

de teoría sobre ellos. Hay infinidad de información en todos los idiomas sobre sistemas de

control basados en PIDs y sus aplicaciones en drones, por lo que no me voy a extender mucho

en este tema. Arduino cuenta con una librería para PID, pero recomiendo no utilizarla e intentar

entender el funcionamiento de estos controladores programándolos nosotros mismo (son muy

sencillos). El objetivo de un PID es conseguir un error entre la consigna de velocidad y la

velocidad real de 0º/s (metros, grados… según la aplicación), es decir, que la velocidad de

rotación real sea igual a la consigna que llega desde el mando en todo momento. Pongamos

como ejemplo uno de los ejes de nuestro drone, por ejemplo el eje Pitch.

Lo primero que hace esta estructura de control es comparar la referencia de velocidad angular

que nos llega desde el mando, en la imagen ‘W Pitch* (mando)’, con la lectura que recibimos del

sensor MPU6050, en la imagen ‘W Pitch (IMU)’. 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:

Por ejemplo, si desde el mando nos llega la consigna de 0º/s de pitch y el sensor MPU6050 está

leyendo que la velocidad real es de +10º/s, la variable ‘Err.1’ tomará el valor -10º/s, es decir,

tenemos un error en la velocidad del eje pitch de -10º/s. Este podría ser un ejemplo de que

debido al viento, nuestro drone se está inclinando en una dirección, cuando lo que queremos es

que se mantenga estable. El objetivo de nuestro PID será hacer que este error sea siempre de 0,

para lo que habrá que actuar sobre los motores, contrarrestando el viento y haciendo que la

lectura del sensor MPU6050 sea de 0º/s (que no gire). El error antes de contrarrestar la

perturbación:

Err.1 = w Pitch* (mando) – w Pitch (IMU) = 0 rad/s – 10rad/s = -10rad/s

El error es enviado al PID y este genera la consigna en micro-segundos acelerando/decelerando

los correspondientes motores hasta contrarrestar la perturbación. Cuando el drone comience a

girar y se vaya corrigiendo la desviación, la variable Err.1 irá disminuyendo hasta convertirse en

0, momento en el que drone habrá detenido la rotación y no haya desviación alguna entre la

consigna que enviamos desde el mando y la rotación real.Una vez corregida la desviación y que

el drone a dejado de rotar, el error baja a 0º/s, CONSEGUIDO, el drone es estable:

Err.1 = w Pitch* (mando) – w Pitch (IMU) = 0 rad/s – 0 rad/s = 0 rad/s

Si por el contrario desde el mando nos llega una referencia de 10º/s, es decir, queremos que

nuestro drone se incline y desplace en su eje pitch, el funcionamiento sería similar. El error

pasaría a ser de 10º/s y el PID aceleraría los correspondientes motores hasta aumentar la

velocidad a 10º/s, es decir, hasta hacer el error cero.

Err.1 = w Pitch* (mando) – w Pitch (IMU) = 10 rad/s – 0 rad/s = + 10rad/s

Tras acelerar los motores, el drone empezará a rotar en el sentido que hayamos indicado hasta

alcanzar la velocidad deseada, momento en el que el error bajará a 0º/s y habrá finalizado la

operación:

Err.1 = w Pitch* (mando) – w Pitch (IMU) = 10 rad/s – 10 rad/s = 0 rad/s

Este error ‘Err.1’ lo recibe el PID y genera una salida ‘Pulso (us)’ en función de los parámetros

Kp, Ki y Kd que hayamos establecido. Simplemente cogemos el error y lo multiplicamos por estos

valores, haciendo más o menos agresivo el control de estabilidad de nuestro drone.

La parte Kp (PID) es proporcional al error, simplemente multiplicamos ambos términos. Si por

ejemplo tenemos un valor de Kp de 10 y tenemos un error de 10º:

10*10º = 100us

Si el motor estaba girando con un PWM de 1.5ms, aceleraría hasta 1.5ms+0.1ms = 1.6ms.

La parte Ki (PID) es proporcional al error que vamos acumulando en cada ciclo. Cogemos el error

actual y lo multiplicamos por el término Ki, pero en cada nuevo ciclo de control sumamos el valor

obtenido en el ciclo anterior. De esta forma conseguimos que el error en régimen permanente

sea de 0.

La parte Kd (PID) es proporcional a la diferencia de error entre ciclos. Sirve para suavizar la

respuesta del control. Más adelante veremos cómo programar los PID en Arduino.

La salida de los PID se da en milisegundos. Hablábamos de cómo variando el tiempo en el que el

PWM está en estado HIGH podíamos aumentar o reducir la velocidad de los motores (En la parte

de los motores). Por esta razón, la salida de los PID se da en milisegundos, porque para corregir

las desviaciones o el error, necesitamos variar el tiempo en el que el PWM está en estado HIGH.

Como ya habréis imaginado, necesitamos un PID para cada eje del drone que queramos

controlar, en nuestro caso 3: Pitch, Roll y Yaw.

Finalmente todas las señales se combinan para generar una señal para cada motor. Al cálculo

de estas cuatro señales he decidido llamarlo MODULADOR. ¡Recordad que estas cuatro señales

se miden en milisegundos!

esc1 = throttle – salida PID pitch + salida PID roll + salida PID yaw

esc2 = throttle – salida PID pitch – salida PID roll – salida PID yaw

esc3 = throttle + salida PID pitch – salida PID roll + salida PID yaw

esc4 = throttle + salida PID pitch + salida PID roll – salida PID yaw

Importante, las señales esc1, esc2, esc3 y esc 4 nunca deben superar los 2ms, que es la consigna

de máxima potencia para los motores. Si alguna de las señales supera esta cifra significa que

algo hemos hecho mal, ¡no podemos enviar al motor una consigna mayor a 2ms! Como veremos

más adelante, vamos a capar por software estas señales para que nunca sean mayores de 2ms,

aunque esta protección nunca debería entrar en funcionamiento si hemos programado bien el

software. Para ello recordad limitar la señal máxima de throttle a un máximo de 1.7ms

aproximadamente, para dejar margen para los PID. Los PID también tendrán que ser limitados

a un valor aproximado de 300us.

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 se indicó en el apartado

correspondiente. 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:

1 float tiempo_ejecucion, loop_timer;

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

void setup() {


}

void loop() {

//

// PROGRAMA PRINCIPAL AQUÍ

//

while (micros() - loop_timer < 6000);

tiempo_ejecucion = (micros() - loop_timer) / 1000;


//

// 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 (más adelante en este apartado veremos

porqué), 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, en este caso con 6ms 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:

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

//===================================================== 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);

26

27

28

29

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

generación 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 ha 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:

1

2

3

4

5

6

7

8

9

10

11

// ============================================== 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




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

1

2

3

4

accion_m1 = esc1 + 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:

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

while (aM1 || aM2 || aM3 || aM4 == true) {

esc_loop_timer = micros();

if (accion_m1 <= esc_loop_timer) { // Motor LOW

aM1 = false;


if (accion_m2 <= esc_loop_timer) { // Motor 2 LOW

aM2 = false;


}


aM3 = false;


}


aM4 = false;


}

}

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. Vayamos a por el modo

estable.

3.6.2 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.

En modo estable contrarrestamos la inclinación

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 (º):

3.7 Código Arduino para control de motores Brushless con ESC

Para generar las salidas PWM de control vamos a utilizar el código que os he dejado más abajo.

Simplemente leeremos el canal throttle del mando RC y haremos girar los motores moviendo el

correspondiente stick del mando (importante haber leído todo sobre el mando RC). 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.

Analicemos el código parte por parte. Lo primero que hacemos al comienzo de cada nuevo ciclo

de 6ms es leer el canal de throttle del mando y escalar las lecturas de forma que al mover el stick

al máximo leamos 2ms y al pasarlo al mínimo 1ms. Aquí se podría utilizar la función map() de

Arduino sin problemas. El siguiente paso es poner las 4 salidas PWM de los 4 motores en estado

HIGH, de esta forma comenzaremos cada ciclo de control con los cuatro PWM en estado

HIGH, sincronizando el control de estabilidad con la generación de las señales PWM:

1

2

3

4

5

6

7

8

while (micros() - loop_timer < usCiclo);


pulsoPotencia = ((PulsoMaxPotencia - PulsoMinPotencia) / (tMinPotencia -

tMaxPotencia)) * ((PulsoPotencia) / 1000.00 - tMaxPotencia) + PulsoMinPotencia;





Como hemos leído en la parte del mando RC y receptor, veréis como el canal throttle de mi

mando está invertido, es decir, con el stick al mínimo leemos 2000us de pulso, y con el stick al

máximo 1000us. Es necesario invertir esta salida. Se podría hacer o bien con la función map() de

Arduino, o bien, como es mi caso, complicándome la vida y programando las ecuaciones por mi

cuenta. Para ello he utilizado la siguiente ecuación, que no es más que la ecuación de una recta

que pasa por dos puntos:

1 pulsoPotencia = ((PulsoMaxPotencia - PulsoMinPotencia) / (tMinPotencia -


Si tenéis el mando invertido como yo, tenéis que dejar los parámetros tal como están:

1

2

const float tMaxPotencia = 1.83;

const float tMinPotencia = 1.12;

Si por el contrario vuestro mando no está invertido, debéis invertir la configuración de esta

forma. Tan simple como esto:

1

2

const float tMaxPotencia = 1.12;

const float tMinPotencia = 1.83;

Más adelante, cuando uséis el software completo y sea necesario configurar los demás canales

(Pitch, Roll y Yaw), usaremos el mismo procedimiento. De todas formas, las implicaciones de no

corregir estos tres canales no son graves, simplemente cuando Ordenemos al drone que avance,

irá para atrás. Pero ya sabéis que hablar de ‘adelante’ o ‘atrás’ en el aire es bastante relativo y

que depende de nosotros.

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. Mas adelante veremos como aprovechar ese mili-segundo para

realizar tareas cortas como leer el mando RC o gestionar los leds.

Después, y aquí viene el truco, sumamos el tiempo de throttle (entre 1ms y 2ms) con el tiempo

total transcurrido desde el inicio del programa (loop_timer). Por ejemplo, si queremos que el

motor gire a velocidad media, moveremos el stick hacia arriba hasta la mitad del recorrido,

leyendo tras el escalado 1.5ms y sumamos este tiempo con el tiempo total transcurrido desde

que hemos iniciado el software (loop_timer), pongamos 1 segundo:

accion_m1 = 1.5ms + 1000ms = 1001.5ms

1 accion_m1 = pulsoPotencia + loop_timer;

2

3

4

accion_m2 = pulsoPotencia + 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 las variables accion_m1, accion_m2, accion_m3 o accion_m4, significará

que el pulso ha estado en estado HIGH exactamente 1.5ms, por lo que es hora de pasarlo a

estado LOW. Arduino no saldrá de este bucle while hasta que las cuatro señales PWM estén en

estado LOW:

De esta forma conseguimos señales PWM de la misma frecuencia del ciclo que hayamos

escogido. Os dejo un pequeño esquema que os ayudará a entender el funcionamiento de esta

parte del código:

Una vez entendido como generar las señales PWM, vamos a proceder a conectar y hacer girar

los motores por primera vez. Cuando conectamos los motores por primera vez hay que seguir

un procedimiento para configurar los ESC. El esquema mínimo necesario se muestra a

continuación:

⊗ MUY IMPORTANTE: hasta que no tengamos claro el funcionamiento del software y

hayamos comprobado que funciona todo bien, NO PONEMOS LAS HÉLICES. Las pondremos

más adelante. Estas pruebas las haremos SIN HÉLICES ⊗

Es necesario configurar los ESC cuando vayamos a utilizarlos por primera vez, para lo que

utilizaremos el código de abajo. Si no dispones de un interruptor puede hacer la conexión entre

la batería y los ESC manualmente, aunque para el montaje final sí que es recomendable disponer

de uno. Os dejo también un vídeo para para que sigáis los pasos de forma mas sencilla:

1. Sin conectar la batería, alimentamos la placa Arduino con USB y cargamos este programa.

2. Ejecutamos el programa sin conectar la batería.

3. Cuando el led 13 se encienda, encendemos el mando y subimos el throttle al máximo.

4. Cuando el led 13 se apague conectamos la batería.

5. El motor emitirá unos pitidos (pi,pi), bajamos el throttle al mínimo.

6. Escucharemos otros pitidos, pipipi… piiii.

7. Terminado, ya podemos apagar todo.

** Este proceso puede ser diferente en función del ESC que hayamos comprado

Esta secuencia hay que hacerla solo la primera vez que conectamos los ESC para poder

configurarlos. Una vez que hemos configurado los ESC vamos a ver cómo podemos hacerlos

girar en función de las órdenes recibidas del mando RC:

1. Cargad el programa y desconectad el cable USB.

2. Conectad la batería y cerrad el interruptor para alimentar la placa Arduino y los motores.

Bajamos el throttle al mínimo y ejecutamos el programa.

3. Cuando el led 13 se encienda, encendemos el mando.

4. Los motores emitirán unos pitidos pipipi… piiiii.

5. Listo, podemos empezar a girar los motores con el mando.

Antes de continuar comprobad que somos capaces de gobernar los motores y que estos giran

de forma controlada. Con el stick en su posición inferior los motores no se mueven, y al mover

el stick los motores giran en proporción.

3.8 Vibraciones de Motor, estudio y su eliminación.

¿Por qué es importante eliminar las vibraciones con la calibración de hélices y motores?

Como ya hemos comentado en la parte del sensor MPU6050, si las hélices no están bien

calibradas, las vibraciones producidas por el movimiento irregular de estas pueden convertirse

en nuestro peor enemigo a la hora de hacer volar nuestro drone. Tened siempre presente el

tema de las vibraciones, ya que si no podéis acabar frustrados y con el drone aparcado el una

estantería y sin haberlo volado nunca. El 100% de las vibraciones son producidas por los

motores y las hélices, de ahí la importancia de la calibración de estos componentes. Son

elementos que giran a gran velocidad y que sustentan todo el peso del drone, por lo que

pequeñas irregularidades en el movimiento de los mismos pueden acarrear grandes vibraciones

que pueden afectar a la estimación de inclinación y hacer que nuestro drone nunca llegue a ser

estable, causando gran frustración en nosotros. Yo mismo estuve a punto de abandonar el

proyecto cuando ya tenía todo listo por no saber cómo identificar este problema, pero

tranquilos, voy a intentar explicar cómo solucionarlo.

Es imprescindible que las hélices y los motores estén completamente equilibrados. La

calibración de estos componentes es un tema vital. Cada pala de la hélice tiene que tener

exactamente el mismo peso que la otra, ya que una desviación de unos poco gramos en una de

las palas girando a 11000rpm genera una cantidad de vibraciones demasiado elevada para

nuestro pequeño y sensible acelerómetro. El problema es que el acelerómetro es

tremendamente sensible a estas vibraciones, por lo que es imprescindible reducirlas al máximo

evitando que se produzcan, para lo que hay que calibrar las hélices lo mejor posible. En caso

contrario, estas vibraciones captadas por el acelerómetro pasan a nuestro control produciendo

lecturas erróneas en el cálculo de la inclinación que pueden llegar a ser de varios grados. Para

que os hagáis una idea, cuando aún no tenía calibradas las hélices y los motores, teniendo el

drone completamente horizontal en el suelo, las vibraciones producidas por las hélices que eran

captadas por el acelerómetro producían un error en la estimación de la inclinación de hasta 7º.

Como vemos en la siguiente ecuación, para calcular el ángulo de inclinación es necesario utilizar

los datos del acelerómetro:

1

2

angulo_pitch = angulo_pitch * 0.999 + angle_pitch_acc * 0.001;


La frecuencia de estas vibraciones es muy fácil de estimar de forma básica. En teoría, una hélice

que gira a 5000rpm, producirá vibraciones a una frecuencia de aproximadamente 83.3Hz:

Velocidad de giro (rpm)/60 = Frecuencia de vibración (Hz)

Asumiendo que los motores giran entre 3000rpm (30% de throttle) y 11000rpm (100% de

throttle), tenemos que enfrentarnos a vibraciones que pueden variar entre 50Hz y 184Hz (esto

es simplemente una aproximación rápida). Una de las soluciones es filtrar la salida del sensor

MPU6050 configurando el filtro LPF (low pass filter) que incorpora el sensor, pero tened cuidado

con filtrar demasiado la salida utilizando frecuencias de corte bajas ya que el filtrado introduce

un retardo en las medidas que puede llegar a los 19ms. Tened en cuenta que el filtrado siempre

nos hará perder prestaciones:

3.8.1 Calibración de hélices y motores

La mejor solución para olvidarnos de las vibraciones es atacarlas en su origen, es decir, calibrar

hélices y motores. La calibración de estos componentes no es una ciencia exacta, no podemos

medir el peso de cada pala por separado por lo que tenemos que recurrir a otras técnicas para

estimar e identificar las vibraciones. Yo utilizo el sketch que os he dejado en el apartado

“CÓDIGO PARA ARDUINO Y MATLAB” más abajo.

Para ello, donde podéis visualizar las variables tanto en el SerialPloter que incorpora

Arduino como en Matlab utilizando el código que os he dejo a continuación:

Es similar al software principal que utilizamos para volar el drone, solo que enfocado a identificar

vibraciones en cada motor, permitiendo analizarlos de forma separada. Simplemente

escogemos el motor que queremos analizar, subimos el throttle, visualizamos las lecturas del

acelerómetro y analizamos los resultados para ver si están o no fuera de rango. Si alguno de los

motores produce vibraciones que estén fuera de rango, tenemos que intentar equilibrar las

palas. La calibración es muy sencilla. Simplemente ponemos un cacho de cinta aislante en una

de las palas y volvemos a hacerlo girar, si las vibraciones se han reducido, podemos ajustar la

cantidad de cinta hasta reducir las vibraciones al mínimo posible. Si por el contrario las

vibraciones aumentan, retiramos la cinta aislante de esa pala y la ponemos en la otra, repitiendo

la prueba y comprobando que efectivamente las vibraciones se han reducido. También podemos

repetir este proceso con los cuatro motores:

La web de ArduPilot recomienda que las vibraciones no superen los ±3m/s² en los X e Y, y que

estén comprendidas entre ±5m/s² y ±15m/s² para el eje Z:

Este proceso de calibración puede parecer algo tedioso pero os aseguro que merece la pena.

A continuación muestro los resultados de la calibración realizada por mi, donde se puede ver

como las vibraciones nos superan los limites marcados (mis resultados están en ‘g‘, pero la

relación es de 10 para compararlos con los resultados de arriba. 1g = 9.8m/s².

El sketch que os dejo, visualiza las lecturas de aceleración en m/s2 , por lo que podréis

comparar los resultados con directamente con las gráficas de ArduPilot sin necesidad de hacer

modificaciones.

Existen otras formas de evitar que las vibraciones pasen al control. De todas formas, el primer

paso siempre es intentar atajar el problema en su origen, equilibrando las cuatro

hélices. Recordad que la calibración de los motores brushless y hélices es siempre lo primero. Es

recomendable también usar plataformas anti-vibración o MoonGel para colocar el sensor

MPU6050 o la controladora, evitando que las vibraciones residuales lleguen a nuestro control.

Si tenemos estos conceptos claros desde un principio vamos a ahorrar tiempo y dinero a la hora

de construir nuestro propio drone.

3.8.2 Código Arduino (Sketch)

Veamos ahora como utilizar el archivo de calibración que os he dejado para descargar

(‘CalibrarESC_v4′).

Antes de continuar con el proceso de calibración es imprescindible haber leído la parte relativa

a los motores y haberlos configurado de forma correcta. El hardware necesario es el siguiente,

que es el mismo que necesitaremos para poder volar el drone. El interruptor secundario es

opcional, sirve para alimentar de forma independiente la parte de potencia de la parte de

control:

⊗ MUY IMPORTANTE: hasta que no tengamos claro el funcionamiento del software y

hayamos comprobado que funciona todo bien, NO PONEMOS LAS HÉLICES. Las pondremos

mas adelante. Estas pruebas las haremos SIN HÉLICES ⊗

El código es similar al que vamos a utilizar para volar el drone. Únicamente he borrado la parte

de los PID y he añadido alguna función para facilitar la visualización de las diferentes

variables. Para poner el código en marcha basta con cargarlo, cerra el interruptor principal para

alimentar la parte de potencia, y después el interruptor secundario para alimentar la parte de

control (todo esto sin hélices). Los motores deberían empezar a pitar y deberían encenderse un

led azul y el LCD (en caso de haberlo instalado, recomendado). Si tenemos el LCD instalado

podremos ver en todo momento en que fase de la puesta en marcha del drone estamos. En caso

de no tener instalado ningún LCD habrá que borrar las partes del código correspondiente. Son

muy fáciles de identificar, todas las lineas de código comienzan por ‘lcd.’:

1

2

3

4

lcd.setCursor(0, 1);

lcd.print("Encender mando");

lcd.setCursor(0, 0);

lcd.print("MODO CALIBR.");

Cuando abrimos el archivo principal, lo primero que veremos son estas lineas de código donde

podemos modificar la frecuencia de PWM, los motores que queramos calibrar y las variables a

visualizar. Si por ejemplo queremos calibrar el motor número 2 y visualizar las vibraciones

captadas por el acelerómetro, deberíamos modificar los parámetros de la siguiente forma

(recordad que es imprescindible haber configurado los motores/ESC de forma correcta antes de

continuar):

1

2

3

4

5

6

7

8

// ===========================================

#define usCiclo 6000 // Ciclo de ejecucion de software en microsegundos

bool m1 = false;

bool m2 = true;

bool m3 = false;

bool m4 = false;

bool VisuAcc = true; // Visualizar aceleración

9

10

bool VisuAng = false; // Visualizar ángulo

// ==========================================

Cargamos el archivo y sin desconectar el cable USB cerramos el interruptor principal para

alimentar los motores. Una vez ejecutar el software se encenderán el led azul, lo que significa

que todo se ha inicializado de forma correcta. A continuación se comprueba que el mando está

encendido para lo que he utilizado el código de abajo. Por seguridad, hasta no encender el

mando y detectar que el throttle está al mínimo el software no saldrá de ese bucle*. Para mas

información, acudir al apartado correspondiente al mando RC. Es importante mantener la

palanca de throttle al mínimo durante todo este proceso:

1

2

3

4

while (pulsoPotencia > 1100 || pulsoPotencia < 900) {



delay(100);

}

* Puede que los valores 1100 y 900 no sirvan para vuestro mando.

Una vez detectado el encendido del mando se calibrarán tanto el mando como el giroscopio y el

acelerómetro. Durante este proceso el led 11 permanecerá encendido. Durante el proceso de

calibración el drone tiene que estar situado en una superficie plana y hay que evitar tocarlo:

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

//============================================= Calibración del mando


calPotencia = calPotencia + PulsoPotencia;

delay(20);

}

calPotencia = calPotencia / 100 - 990; // MIRAR cuantos microsegundos son con la

palanaca para abajo

//============================================= Calibración del giroscopio


MPU_6050();

gyro_X_cal += gx;

gyro_Y_cal += gy;

gyro_Z_cal += gz;

delayMicroseconds(1000);

}

gyro_X_cal = gyro_X_cal / 3000;

gyro_Y_cal = gyro_Y_cal / 3000;

gyro_Z_cal = gyro_Z_cal / 3000;

//============================================= Calibración del acelerómetro


22

23

24

25

26

27

28

29

MPU_6050();

angle_pitch_acc_cal += ax;

angle_roll_acc_cal += ay;

angle_yaw_acc_cal += az;

}

angle_pitch_acc_cal = angle_pitch_acc_cal / 3000;

angle_roll_acc_cal = angle_roll_acc_cal / 3000;

angle_yaw_acc_cal = angle_yaw_acc_cal / 3000;

Finalmente se ponen en marcha los motores. Deberíamos oír unos pitidos como pipipi…piii que

indican que los motores están armados y listos para girar, aunque este sonido puede variar en

función de cada motor y ESC. El software se quedará en ese bucle hasta que indiquemos que

avance girando la palanca de Roll hacia la derecha. Este paso se puede omitir borrando la

correspondiente parte del código:

1

2

3

4

5

6

7

8

9

10

11

while (wRoll < 20) {



wRoll = ((wMaxRoll - wMinRoll) / (tMax - tMin)) * ((PulsoRoll - calRoll) / 1000.00 - tMin)

+ wMinRoll;

digitalWrite(3, HIGH); //Motor 1




delayMicroseconds(pulsoPotencia);

digitalWrite(3, LOW); //Motor 1


12

13

14



delayMicroseconds(usCiclo - pulsoPotencia);

}

Una vez hecho todo esto, empezará a correr el software principal y el led 11 empezará a

parpadear. Conviene hacer una serie de pequeñas comprobaciones para asegurarnos de que

todo funciona correctamente (todo esto sin hélices):

Comprobad que los motores se gobiernan adecuadamente desde el mando. Con el stick en su

posición inferior los motores no se mueven, y al mover el stick los motores giran en proporción.

Comprobad que las lecturas que obtenemos del acelerómetro son correctas. En torno a 0m/s²

para los ejes X e Y, y en torno a 9.8m/s² para el eje Z. Movemos el drone con la mano y

comprobamos que los valores cambian.

Comprobamos que la estimación del ángulo se hace de forma correcta. Situando el drone en el

suelo deberíamos leer 0º de inclinación y a medida que lo movemos el angulo debería cambiar.

Comprobad también que sucede si reiniciamos la placa Arduino mientras estamos corriendo el

software, se de casos en los que los motores pierden el control y se aceleran durante los dos

segundos que dura el reinicio (peligroso).

Una vez que hemos hecho seguido el procedimiento y somos capaces de girar los motores de

forma controlada usando el mando, solo entonces pondremos las hélices.

3.8.3 Montaje de las hélices

⊗ Esta es la primera vez que vamos a poner en marcha los motores con las hélices puestas.

MUCHO CUIDADO. Aseguraos de que el drone está firmemente sujeto al suelo y de que no hay

peligro de tocar las hélices ⊗

El siguiente paso es instalar las hélices. Antes de continuar, acordaos de que cada motor tiene

que girar en un sentido concreto como hemos visto en la zona dedicada a los motores. Conviene

hacer el calibrado de hélices y motores con los motores girando en el sentido correcto.

Os habréis fijado en que el fabricante nos provee con dos hélices diferentes además de con el

motor. Esto es debido a que dependiendo del sentido de giro del motor tendremos que utilizar

o la una o la otra para que el empuje sea siempre hacia abajo (recordad que tenemos dos

motores girando en sentido horario y dos girando en sentido anti-horario). Para saber que hélice

corresponde con cada sentido de giro tenéis que fijaos en la forma de la propia hélice, donde la

‘parte lisa’ siempre tiene que estar de cara al movimiento, y la ‘muesca’ detrás:

Comprobad también que no las estáis poniendo boca abajo. La serigrafía con el tipo de hélice

(en mi caso 10×4.5R), siempre hacia arriba.

Una vez instaladas hacedlas girar a baja velocidad. En seguida os daréis cuanta de hacia donde

empuja el motor, tenéis que sentir que el aire sale hacia abajo.

Una vez identificadas que hélices corresponden con cada motor, es hora de que empezáis con

los test de vibraciones y el proceso calibración. Recordad amarrar bien el drone para que no

despegue y tomad todas las precauciones necesarias. Corregid las vibraciones de cada motor

por separado con cinta aislante como hemos explicado mas arriba y finalmente comprobad los

cuatro motores girando a la vez.

3.8.4 Código para Arduino y Matlab.

Para el siguiente ejemplo vamos a crear 4 variables aleatorias utilizando la función random() de Arduino y después las enviaremos al PC donde las visualizaremos utilizando Matlab. El código Arduino es muy sencillo de entender, simplemente hay que enviar las 4 señales que queramos visualizar en Matlab formando un vector como hago en el siguiente sketch. Para no consumir demasiado tiempo en el envío de los datos solo enviamos una variable cada ciclo: Y el código de Matlab aquí:

Y esto es lo que veríamos en Matlab:

Lectura de variables en Matlab

3.9 Resumen y Puesta en funcionamiento final del Dron.

Antes de volar el drone es necesario asegurar que el software completo funciona

correctamente. En esta ocasión, vamos a ver cómo hacer estas comprobaciones de forma rápida

y sencilla. Las variables que vamos a verificar son: la salida del sensor MPU6050, el mando RC y

las cuatro salidas a los motores. Todas estas comprobaciones las haremos sin conectar la batería,

únicamente alimentando la placa Arduino a través del cable USB.

En primer lugar, conviene recordar el convenio de signos que hemos tomado a la hora de

construir y programar el drone. Recordad que la orientación en el frame del sensor MPU6050

define la parte delantera/trasera y la parte izquierda/derecha del drone, y también la

numeración de los motores:

Si por la razón que sea vuestro hardware es diferente a este, es decir, si la numeración de los

motores respecto a la orientación del sensor MPU6050 no se corresponde con la de la imagen,

o si por error habéis conectado los motores a alguna salida digital diferente a la que se muestra,

habrá que hacer algunas modificaciones en el código. Concretamente habrá que cambiar los

signos de las siguientes ecuaciones:

1

2

3

4

5

// Modulador





Estas ecuaciones, que he llamado Modulador, son las encargadas de actuar sobre determinados

motores en función de inclinación del drone. Si el control de estabilidad ordena acelerar o

decelerar alguno de los motores para mantener el drone estable, pero esos motores están

conectados donde no deberían (por ejemplo, en el brazo opuesto frame), el drone no podrá ni

siquiera despegar, porque el motor que estamos acelerando estará en el lugar equivocado. En

vez de contrarrestar la inclinación estaremos ayudando a que esta se acentúe, por lo que todo

acabará en una vuelta de campana por parte del drone.

Para asegurarnos de que todo está bien antes de empezar a volar, vamos a realizar 3 sencillas

pruebas. Si pasáis de forma satisfactoria la prueba número 3 que se muestra a continuación, el

montaje y la integración con el software se ha hecho de forma correcta. Si no, habrá que

modificar los algunos signos del código que os acabo de mencionar.

Si bien superando la prueba número 3 la 1 y la 2 quedan superadas de forma indirecta,

recomiendo hacer las tres en el orden que se indica.

3.9.1 Comprobaciones

Después de mencionar esto, pasamos directamente a hacer las comprobaciones necesarias. Nos

vamos a centrar en las tres comprobaciones más críticas, sin las cuales no es posible volar el

drone.

3.9.1.1 Comprobación Hardware iniciales:

En primer lugar, asegurad que los motores giran en la dirección correcta y que las hélices están

bien colocadas en función de la dirección de giro. Es muy importante. Tenéis más información

en la parte dedicada a la calibración de ESC y motores.

Cada motor gira en un sentido concreto según su posición en el frame. En función del sentido

de giro de cada motor, hay que utilizar una hélice o la otra. En el paquete siempre vendrán dos

tipos de hélices, uno para cada sentido de giro del motor. Comprobad también que no las estáis

poniendo boca abajo. La serigrafía con el tipo de hélice (en mi caso 10×4.5R), siempre hacia

arriba.

Prueba 1 ⇒ MPU6050

El primer lugar vamos a comprobar el funcionamiento del sensor MPU6050. Comprobaremos

que la estimación del ángulo de inclinación (angulo_pitch y angulo_Roll) se hace de forma

correcta y de que la orientación del sensor en el frame es el adecuado.

Para ello, cargamos el software principal y activamos el modo de visualizaciones:

1 int visu = 0; // En vuelo las dos a 0!!

En este caso queremos visualizar la inclinación:

1 int VisuSelect = 3; // --> 0: mando, 1: giro, 2: acc, 3: ang, 4: esc, 5: mando RC

Pestañas del software donde se encuentran esas variables

En primer lugar, aseguraos de que la estimación de la inclinación se hace bien, que con el drone

en una superficie plana el valor de las variables angulo_pitch y angulo_Roll es de 0º, y que a

medida que lo inclinamos la estimación calcula el ángulo de inclinación bien.

Con el drone en una superficie plana la estimación de la inclinación es de 0º, inclinando el drone

la estimación varía

Después comprobaremos que los signos en función de a qué lado orientemos el drone son

correctos. Si inclinamos el drone hacia adelante, es decir, en dirección a la ‘flecha Y’ del sensor

MPU6050, la inclinación en ese eje tiene que ser negativa, mientras que, si lo inclinamos hacia

atrás, la inclinación será positiva:

Inclinando el drone hacia ‘adelante’, inclinación en el eje Pitch negativa

En el eje Roll en cambio, inclinando el drone a la derecha obtenemos un ángulo positivo en ese

eje, mientras que si lo inclinamos a la izquierda obtenemos una estimación negativa: Si alguno

de estos signos es diferente al aquí mencionado, habrá que modificar los signos en el software,

cosa que no es tan evidente de hacer. Significará que no hemos orientado el sensor MPU6050

en el frame de forma adecuada. Más adelante veremos cómo solucionarlo.

Inclinando el drone hacia la ‘derecha’, inclinación en el eje Roll positiva

Prueba 2 ⇒ Mando RC

Comprobar el funcionamiento del mando RC es igual de importante. A continuación, vamos a

testear las variables wPitchConsigna, wRollConsigna, wYawConsigna y PotenciaFilt.


Moved uno por uno los stick del mando y analizar los resultados por el monitor serie. Más que

en el valor final, fijaos en el signo (si es positivo o negativo). Si alguno de los signos es diferente

al mencionado abajo, habrá que modificar la parte del código donde se declaran las variables de

ese canal:

Moviendo stick de Pitch hacia arriba, wPitchConsigna = -20º aprox.

Moviendo stick de Pitch hacia abajo, wPitchConsigna = +20º aprox.

Moviendo stick de Roll hacia la derecha, wRollConsigna = +20º aprox.

Moviendo stick de Roll hacia la izquierda, wRollConsigna = -20º aprox.

Moviendo stick de Yaw hacia la derecha, wYawConsigna = +50º aprox.

Moviendo stick de Yaw hacia la izquierda, wYawConsigna = +50 aprox.

Modificar el código para solucionar este problema es tan fácil como invertir estos dos

parámetros del canal que esté invertido. De esto:

1

2

3

// MANDO PITCH

const int wMaxPitch = -75; // <-- Si tenéis la entrada Pitch invertida sustituid este valor

const int wMinPitch = 75; // <-- por este y viceversa

A esto:

1

2

3

// MANDO PITCH

const int wMaxPitch = 75; // <-- Si tenéis la entrada Pitch invertida sustituid este valor

const int wMinPitch = -75; // <-- por este y viceversa

Las implicaciones de no hacer esta modificación no son tan graves, simplemente, cuando

ordenáramos al drone ‘avanzar’, este retrocedería. Nada grave, pero conviene corregirlo.

Un punto muy importante para tener en cuenta en los canales Pitch, Roll y Yaw: hay que

asegurar que con los sticks en tu posición central (sin moverlos), las variables wPitchConsigna,

wRollhConsigna y wYawConsigna son cero. Exactamente cero. Si no, el drone podría desplazarse

hacia un lado sin que nosotros lo ordenáramos, simplemente porque con el stick en su posición

central estamos leyendo valores diferentes de cero. Para asegurar que esto se cumple, he

añadido estas líneas de código, para hacer forzar a cero la señal siempre que estemos dentro de

un rango determinado:

1

2

3

if (wRollFilt < 3 && wRollFilt > -3)wRollConsigna = 0;

if (wYawlFilt < 3 && wYawlFilt > -3)wYawConsigna = 0;

if (wPitchFilt < 9 && wPitchFilt > -9)wPitchConsigna = 0; // '9' por un error de fábrica de

mi mando

Para la consigna de throttle o PotenciaFilt, basta con comprobar que con el stick al mínimo

obtenemos valores cercanos a 950us y con el stick al máximo valores cercanos a 1800us (el valor

de throttle máximo está limitado a 1800us para dar margen al control de estabilidad):

Throttle al mínimo y al máximo

Este si es un punto crítico. Hay que asegurar que con el stick al mínimo la señal de throttle es

cercana a 900-1000us. Si tuviéramos este canal invertido, es decir, si con el stick al mínimo

obtuviéramos lecturas de 1800us, el drone despegaría a máxima potencia nada más entrar al

loop principal, cosa que queremos evitar.

De todas formas, el software está preparado para evitar que la señal enviada a los motores con

el Stick de throttle al mínimo sea inferior a un valor determinado. La señal mínima que enviamos

a los motores viene definida por este valor, que tiene que ser menor al valor con el que hayamos

calibrado los motores, si no, los motores girarán o intentarán girar con el stick de throttle al

mínimo. Estas variables se encuentran en la pestaña ModuladorPWM del software:

1

2

3

4

if (esc1 < 1000) esc1 = 950;

if (esc2 < 1000) esc2 = 950;

if (esc3 < 1000) esc3 = 950;

if (esc4 < 1000) esc4 = 950;

Si al acabar las pruebas, cuando conectéis la batería, los motores giran un poco o intentan girar

con el Stick de throttle al mínimo, reducir ese valor de 950 a 920, por ejemplo. Hay que conseguir

que estén completamente parados.

Prueba 3 ⇒ Control de estabilidad

Llegamos a la parte más importante. Si la estimación de la inclinación y la lectura del mando se

hacen de forma correcta, vamos a comprobar que el control d estabilidad funciona de forma

adecuada y de que las señales esc1, esc2, esc3 y esc4 se ajustan de forma correcta en función

de la inclinación del drone. Para ello, es imprescindible haber cableado los motores a las salidas

digitales correctamente. Es decir, que la señal esc1 actúe sobre el motor 1, la señal esc2 sobre

el motor 2… De nada servirá hacer estas pruebas si después hemos cableado la señal esc1 al

motor número 2 por error.

1

2

3

4

5

// Declarar motores

pinMode(3, OUTPUT); // Motor 1 – señal esc1




Es muy importante que tengáis identificado claramente cada motor y a que salida digital está

cableado.

Para la primera fase no es necesario utilizar el mando RC, simplemente vamos a ir inclinando el

drone en sus diferentes ángulos con la mano, y vamos a analizar las salidas esc1, esc2, esc3 y

esc4. No conectaremos la batería en ningún momento. Únicamente alimentaremos la placa

Arduino con el cable USB. En este caso queremos visualizar las señales esc1, esc2, esc3 y esc4:


Por seguridad, el control de estabilidad se activa solo cuando subimos el throttle por encima de

los 1300us, por lo que para visualizar el comportamiento de esc1, esc2, esc3 y esc4 tendréis que

subir la stick por encima de este valor.

El trabajo del control de estabilidad es mantener el drone siempre estable. Si inclinamos el drone

hacia adelante, es decir, en dirección a la ‘flecha Y’ del sensor MPU6050, la consigna de los

motores 1 y 2 tiene que aumentar, mientras que la de los motores 3 y 4 tiene que reducirse. Si

accionáramos por error cualquier otro motor que no fueran estos, estaríamos ayudando a que

continuara inclinándose, y perderíamos el control del done. Esta es la prueba más importante

de todas:

Inclinando el drone hacia ‘adelante’, las variables esc1 y esc2 aumentan. Las variables esc3 y

esc4 se reducen. Fiaos en la numeración de los motores en el frame.

Esta es la base del control de estabilidad y es una NORMA que hay que cumplir SIEMPRE: el

sensor detecta inclinaciones, y el control las contrarresta ajustando las señales esc1, esc2, esc3

y esc4.

Si en cambio movemos el drone hacia la ‘derecha’, la consigna de los motores 1 y 4 tiene que

aumentar, mientras que la de los motores 2 y 3 a de reducirse:

Inclinando el drone hacia la ‘derecha’, las variables esc1 y esc4 aumentan. Las variables esc2 y

esc3 se reducen. Fiaos en la numeración de los motores en el frame.

Comprobad esto en todos los ejes y en todas direcciones. Como veis, es la fase final del control

de estabilidad. Estamos comprobando que las señales esc1, esc2, esc3 y esc4 son ajustadas de

forma correcta en función de la inclinación del drone.

En caso de no haber fijado la posición del sensor MPU6050 de forma correcta, o de haber

empezado con el blog con el drone ya montado, en definitiva, si no hemos superado las pruebas

que acabamos de hacer en este punto 3, será necesario modificar los signos de esas ecuaciones

hasta hacer que la pruebas que hemos hecho más arriba se cumplan. Esto es imprescindible.

Independientemente de cómo hayamos montado el hardware, siempre tendremos que

comprobar esto y asegurarnos de que funciona correctamente. Las señales esc1, esc2, esc3 y

esc4 siempre tienen que ajustarse de forma correcta en función de la inclinación del drone.

Modificar esta parte del Software es bastante sencillo, pero hay que saber lo que se está

haciendo. Si inclinando el drone en el eje Pitch hemos visto que se aceleran los motores que no

debería, habrá que modificar el signo del modular PID_pitch_w. Si hemos visto que sucede en el

eje Roll, los signos dl parámetro PID_roll_w. Así hasta conseguir cumplir la NORMA del punto 3.

Si por ejemplo es el eje Pitch el que está dando problemas, invertimos los signos

correspondientes a ese eje y volvemos a probar:

1

2

3

4

5

// Modulador





1

2

3

4

5

// 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

**Comentario: Realmente, la posición del sensor MPU6050 en el frame es indiferentes, siempre

que consigamos que se cumplan las pruebas que acabamos de hacer en este punto 3. En este

blog, en la parte dedicada al sensor MPU6050, insistíamos en orientar el sensor de una forma

determinada. Con esto conseguimos hacer más fácil la utilización del software principal y no

tener que modificarlo. Simplemente, hemos fijado la posición del sensor en una orientación

determinada para poder utilizar las ecuaciones que se incluyen en el Modulador del software

principal sin modificarlas.

La misma lógica la vamos a utilizar para comprobar que el drone actúa según lo esperado ante

las órdenes recibidas desde el mando RC. Para ello vamos a dejar el drone posado sobre una

superficie plana y vamos a coger el mando RC.

Si movemos el stick del canal Pitch hacia arriba, estamos ordenando al drone avanzar (hacia

adelante). Para ello, según el convenio de signos que hemos escogido, la consigna de los motores

3 y 4 tiene que aumentar, mientras que la de los motores 1 y 2 tiene que reducirse. De esta

forma, el done se inclinará en el eje Pitch y avanzará en esa dirección:

Mover stick de Pich hacia arriba

Subiendo el stick asociado al eje Pitch hacia arriba (ordenando a drone ir hacia adelante), las

variables esc3 y esc4 aumenta las variables esc1 y esc2 se reducen.

Lo mismo sucede con el eje Roll. Si movemos el stick del canal Roll hacia la derecha, estamos

ordenando al drone desplazarse en esa dirección. Para ello, según el convenio de signos que

hemos escogido, la consigna de los motores 2 y 3 tiene que aumentar, mientras que la de los

motores 1 y 4 tiene que reducirse. De esta forma, el done se inclinará en esa dirección en el eje

Roll y avanzará en esa dirección:

Mover stick Roll hacia la derecha

Subiendo el stick asociado al eje Roll hacia la derecha (ordenando a drone ir hacia la derecha),

las variables esc2 y esc3 aumentan las variables esc1 y esc4 se reducen.

Comprobad esto en todos los ejes y en todas direcciones.

El eje Yaw es más es algo más complicado de comprender. Controlando el eje Yaw el drone no

se desplaza, simplemente gira sobre su eje central. Por ello, recomiendo en un principio no hacer

comprobaciones. Simplemente volad el drone. Si este empieza a rotar sobre su propio eje, los

signos del eje Yaw estarán invertidos. Solucionarlo es muy sencillo. Simplemente tenéis que

modificar los signos en el modulador. De esto:

1

2

3

4

5

// Modulador





A esto:

1

2

3

4

5

// 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

Otros:

El led 13, que indica que la ejecución del código se hace dentro de los tiempos requeridos, nunca

debería encenderse. Si se enciende, significa que el tiempo que necesita la placa Arduino para

ejecutar el código es mayor que el tiempo de ciclo que hemos puesto (uCliclo). Si no habéis

modificado el tiempo de ciclo ni añadido nada al código, esto no debería pasar nunca, el led 13

estará siempre apagado.

Eso es todo… �

Recomiendo dedicar algo de tiempo al tema de las comprobaciones y asegurar que todo

funciona como es debido. A nadie le gusta estrellar el drone en la primera prueba de vuelo.

Además de los puntos que hemos mencionado, se pueden comprobar otra serie de elementos

menos críticos, como la lectura de tensión de la batería.

Un tema por el que muchos me preguntáis es el de ajustar los controladores PID.

Desafortunadamente, es necesario hacer el ajuste a prueba y error mientras volamos. Para

ajustar los valores de los controladores en base a simulación sería necesario conocer el modelo

matemático completo del drone, cosa que es extremadamente complejo y que se escapa

completamente del objetivo de este blog. Por lo que no queda otra que justar los parámetros

on-line.

No hay una forma fija de configurar los parámetros de los PID. Estos dependen del tamaño, del

peso, de la construcción… que hayamos llevado a cabo. Cada drone necesita unos parámetros

específicos para volar de forma suave. Como consejo, recomendaría dividir entre dos los valores

que utilizo yo e ir incrementando su valor poco a poco a medida que vamos haciendo pruebas

de vuelo, hasta alcanzar el punto óptimo.

Los parámetros que tenéis que modificar son los siguientes (en el ejemplo solo se muestran los

parámetros de uno de los PID pero hay que modificar todos):

1 float Kp_pitch_w = 1.9, Ki_pitch_w = 0.07, Kd_pitch_w = 12;

Por último, cuando terminéis con las pruebas, recordad poner a cero la opción de

visualizaciones.

Documents

Guía Montaje Drone) · 1. Conceptos generales sobre Drones Para el montaje de nuestro Dron, vamos a necesitar conocer cómo funciona un Dron, porqué gira a un lado u otro, sobre