Procesador aeronáutico de trayectoria y orientación

Cu so: 2020 - 2021
Fecha: Junio, 2021>
Alumno/Alumna: Elgezabal Núñez, A i z
Di ec o /Di ec o a: O iz Ál a ez-Cien uegos, Ja ie
GRADO EN INGENIERÍA ELECTRÓNICA
INDUSTRIAL Y AUTOMÁTICA
TRABAJO FIN DE GRADO
PROCESADOR AERONÁUTICO DE
TRAYECTORIA Y ORIENTACIÓN
1
Índice
1. In oducción.................................................................................................................................................................. 3
2. Con ex o .......................................................................................................................................................................... 3
3. Obje i os y plani icación empo al ..................................................................................................................... 4
4. Desa ollo....................................................................................................................................................................... 5
4.1 Ae omodelo ........................................................................................................................................................... 5
4.1.1 Funcionamien o de un Ae omodelo de ala ija ............................................................................. 5
4.2 Pe il ae odinámico ....................................................................................................................................... 7
4.3 A iónica ................................................................................................................................................................ 13
4.3.1 Ha dwa e .................................................................................................................................................... 13
4.3.2 So wa e ....................................................................................................................................................... 20
5 Resul ados.................................................................................................................................................................... 42
5.1 Validación del algo i mo ............................................................................................................................... 42
5.1.1 XSens MTi-7 ............................................................................................................................................... 42
5.1.2 MPU-9250 ................................................................................................................................................... 42
5.1.3 P ueba en banco de p uebas ............................................................................................................... 43
5.1.4 P uebas dinámicas .................................................................................................................................. 45
5.2 Tiempos de los subp ocesos........................................................................................................................ 46
5.3 P ueba con ae omodelo ................................................................................................................................ 48
6 Conclusiones ............................................................................................................................................................... 52
6.1 Au onomía ........................................................................................................................................................... 52
6.1.1 Tu bo an o Tu boje ............................................................................................................................... 52
6.1.2 Paneles sola es ......................................................................................................................................... 52
6.2 Ae omodelo ........................................................................................................................................................ 53
6.2.1 Es abilidad .................................................................................................................................................. 53
6.2.2 Sus en ación, en ada en pé dida e hipe sus en ado es ........................................................ 53
6.2.3 Velocidad ..................................................................................................................................................... 53
6.3 Posicionamien o global ................................................................................................................................. 54
6.3.1 P ecisión ...................................................................................................................................................... 54
6.3.2 Pé dida de la señal .................................................................................................................................. 54
6.4 Algo i mos de con ol..................................................................................................................................... 55
6.4.1 Con ol Guiado .......................................................................................................................................... 55
2
6.4.2 Con ol de ayec o ia............................................................................................................................ 55
6.4.3 Modo Repe ido ........................................................................................................................................ 55
6.5 Lazo de con ol de elocidad ae odinámica ......................................................................................... 56
7 Bibliog a ía .................................................................................................................................................................. 57
7.1 Ae odinámica y Ae onáu ica ....................................................................................................................... 57
7.2 So wa e y Con ol ........................................................................................................................................... 57
7.3 Ha dwa e ............................................................................................................................................................. 57
8 Anexos ........................................................................................................................................................................... 58
8.1 Mic omod Es ánda M.2 ................................................................................................................................ 58
8.2 Fil o de o ien ación ....................................................................................................................................... 69
8.2.1 Rep esen ación po “Cua e niones” ................................................................................................ 69
8.2.2 O ien ación po a io angula ............................................................................................................ 71
8.2.3 O ien ación po obse ación ec o ial ........................................................................................... 71
8.2.4 Algo i mo de usión de il os ............................................................................................................. 74
8.2.5 Compensación de la dis o sión magné ica ................................................................................... 76
8.2.6 Compensación de la de i a gi oscópica ......................................................................................... 76
8.3 XSens MTi-7 ........................................................................................................................................................ 78
8.3.1 P ocedimien o de en ío SPI (MTi 1 – se ies) ............................................................................... 78
8.3.2 MTSSP P o ocolo se ial sínc ono ...................................................................................................... 78
8.4 Mic ocon olado ATMEGA2560 ............................................................................................................... 83
8.4.1 Diag ama de bloques del mic ocon olado ................................................................................ 83
8.4.2 Pe i é ico SPI ............................................................................................................................................. 84
8.4.3 Pe i é ico USART ..................................................................................................................................... 88
8.4.4 Especi icaciones del TWI compa ible con I2C ............................................................................. 94
8.5 Módulo ecep o GNSS ................................................................................................................................... 99
8.6 MARG .................................................................................................................................................................. 112
3
1. In oducción
El equipo BiSky Team de la Escuela de Ingenie ía de Bilbao se plan ea el uso de
ae omodelos au ogobe nados pa a man ene una supe isión ac i a du an e los lanzamien os
de cohe es, así como pa a el econocimien o de á eas poco accesibles con in ención de
localiza los cohe es cuando ya hayan a e izado.
La idea del p oyec o es la siguien e: la a iónica den o del ae omodelo, median e una
unidad de ins umen ación y un sis ema G.N.S.S., es capaz de conoce y co egi an o su
posición como o ien ación y elocidad espec o al plano e es e, ac uando sob e el mo o y
las supe icies de con ol. An es del despegue se ca ga una lis a de pun os G.N.S.S. po los que
se enca ga á de na ega en o den de o ma au omá ica. En caso necesa io, el supe iso puede
cambia el modo de uelo y maneja emo amen e la ae ona e. Es a ae ona e se i á de
pla a o ma pa a di e en es ca gas ( adio epe ido es, cáma as, expe imen os… e c.) du an e su
uelo.
2. Con ex o
Du an e los lanzamien os de cohe es de BiSky Team, el equipo se ha encon ado con e os a
la ho a de ecupe a los lanzado es. El e o p incipal a supe a es la pé dida de cobe u a del
adioenlace de eleme ía. Cuando el cohe e cae es posible que caiga de ás de un obs áculo y
que és e p oduzca somb a (elec omagné ica). El cohe e ansmi e su posición GNSS ía enlace
de eleme ía, es po eso que se p esen a es a solución.
El ae omodelo con con ol au omá ico puede ealiza la búsqueda del apa a o una ez haya
caído, pa a así se i de pla a o ma a un epe ido en e el cohe e y la es ación de ie a. Una
ez encon ado el apa a o, el ae omodelo puede pe manece en el ai e sin necesidad de
in e ención, simpli icando y acili ando así la labo de búsqueda y ecupe ación.
Ilus ación 1: P opues a al p oblema del adioenlace
4
3. Obje i os y plani icación empo al
En es e p oyec o, cen ado en la elec ónica indus ial y au omá ica, los obje i os gene ales
son:
1. Diseña la a iónica de un ae omodelo.
1. Escoge el ipo de ae omodelo.
2. Escoge la ins umen ación.
3. Escoge y p og ama el il o digi al de o ien ación.
4. Desa olla un p og ama uncional y uni e sal.
Se comienza ecopilando in o mación eó ica ace ca de ae odinámica gene al, de la
ins umen ación u ilizada en la elec ónica de a iación y mé odos de o ien ación elec ónicos.
Se busca una elación de comp omiso en e los mé odos más económicos y los más
compu acionalmen e e icien es.
Una ez escogidos los mé odos a u iliza , se accede a in o mación sob e ae ona egación y
se simpli ica el p oblema. Es a simpli icación ae consigo una di icul ad añadida, pues o que el
sis ema de e e encia del sis ema de o ien ación y el de ayec o ia no ienen po qué se el
mismo, po lo an o, se á necesa io busca una o ma de elaciona , di ec a o indi ec amen e,
ambos sis emas de e e encia.
En la siguien e e apa se diseñan las maniob as de uelo y el/los lazo(s) de con ol que
gobe na án la ae ona e. El modelo del a ión se ha á lo más simple y uni e sal posible, con el
in de que pueda se eu ilizado en u u as simulaciones ex e nas a es e p oyec o.
Después, se diseña un ae omodelo sencillo, cuyas p incipales ca ac e ís icas se án su
es abilidad y len a eacción. Es o acili a á las p ime as p uebas y la a inación de los PIDs que
se p og ama án.
Finalmen e, se e mina el so wa e y se ealizan an o p uebas es á icas como dinámicas
del p o o ipo.
Ilus ación 2: Diag ama Gan , en semanas

5
4. Desa ollo
El desa ollo del p oyec o se di ide en 2 pa es p incipalmen e: p ime o el diseño y
desa ollo de un ae omodelo de ala ija, segundo el diseño y p og amación de la a iónica que
gobie na el apa a o.
4.1 Ae omodelo
Con la in ención de pa i de un ae omodelo cuya es abilidad es conocida, se plan ea
diseña desde ce o un ae omodelo de ala ija pa a baja elocidad de uelo. Pa a ealiza el
diseño se ealiza una i e ación de diseño y en unción de los esul ados de las simulaciones, se
modi ican las di e en es dimensiones y posiciones de las supe icies ae odinámicas. Es o inicia
un nue o p oceso i e a i o has a el momen o en que el ae omodelo cumpla los equisi os
exigibles pa a posibili a el desempeño de sus unciones.
Pa a simula el diseño del ae omodelo se u iliza el p og ama XFLR5. P ime o se in oducen
di e en es pe iles ae odinámicos pa a analiza los median e el p og ama. Después, los da os
ecabados po el p og ama se u ilizan pa a ealiza las simulaciones pe inen es a la ho a de
diseña el a ión. An es de empeza con el diseño del ae omodelo es impo an e en ende el
uncionamien o de los a iones.
4.1.1 Funcionamien o de un Ae omodelo de ala ija
También conocidos como ae oplanos o a iones, son ehículos capaces de ola siendo más
pesados que el ai e, cuya ca ac e ís ica p incipal pa a pode hace lo es el uso de alas ijas. La
azón de que las alas pe mi an al a ión ola es el concep o ae odinámico de “Downwash”.
En ae odinámica en gene al se de ine downwash como el cambio en la di ección del ai e
des iado po la acción ae odinámica de una supe icie (como la pala del o o de un
helicóp e o o el ala de un a ión en mo imien o), como pa e del p oceso de p oducción de
sus en ación. Es po eso que un ala debe e se como una he amien a diseñada pa a lanza el
ai e a su paso hacia abajo. Es o es algo bas an e in ui i o si mi amos la o ma del ala y emos
lo que ocu e con el e ec o Coanda en una cucha illa.
Ilus ación 3: E ec o Coanda demos ado con una cucha a y agua.
6
“El e ec o Coanda es el enómeno ísico po el cual una co ien e de luido, gaseosa o líquida,
iende a se a aída po una supe icie ecina a su ayec o ia. El é mino ue acuñado po
Albe Me al en hono al ingenie o ae onáu ico umano Hen i Coandă, quien descub ió el
e ec o en su p o o ipo de un a ión a eacción.” – Wikipedia 06/04/2021
Que un a ión se sos enga en el ai e o que un pe il ae odinámico gene e sus en ación es un
ejemplo pe ec o de la aplicación de la e ce a ley de mo imien o de New on: Al c ea el lujo
descenden e, como eacción apa ece una ue za de igual magni ud y sen ido opues o, que se
aplica sob e el a ión: es la ue za de sus en ación. La sus en ación es una ue za y, como al, se
puede medi aplicando la segunda ley de New on.
La sus en ación p oducida po un ala es p opo cional a la can idad de ai e que és a des ía
po unidad de iempo, mul iplicada po la elocidad e ical del ai e que ha sido des iado. De
o ma simila al p incipio de ope ación de un helicóp e o, uno puede aumen a la sus en ación
de un ala aumen ando la elocidad e ical del ai e, la can idad de ai e que se des ía, o una
combinación de las dos.
Ilus ación 4: P incipio del Downwash ilus ado
7
4.2 Pe il ae odinámico
P e io al diseño del ae omodelo es necesa io escoge los pe iles ae odinámicos que se
u iliza án en sus alas, imón de di ección y imón de p o undidad.
4.2.1 Nomencla u a de los pe iles NACA
Exis en di e sas publicaciones donde se ca alogan las p opiedades de amilias de pe iles y
se acili a in o mación sob e sus ca ac e ís icas ae odinámicas. Posiblemen e los pe iles más
u ilizados en ae onáu ica sean los pe iles NACA, cuya nomencla u a de 4 ci as se explica en la
abla siguien e, ecogida de [1].
Familia
Pe iles NACA de cua o ci as
Espeso
Es la misma ley de espeso es que la de pe iles Cla k Y y Gö ingen 398, con
un ac o de a inidad que p opo ciona el alo del espeso ela i o.
Cu a u a
Dos pa ábolas de segundo g ado angen es en el pun o de angen e
ho izon al
Nomencla u a
NACA XYZZ (ejemplo NACA 2415)
X
100 /c donde es la lecha máxima de la cu a u a y c la cue da.
Y
10x /c donde x es la posición de la lecha máxima de la cu a u a.
ZZ
100 e/c, donde e es el espeso máximo y c la cue da.
Ilus ación 5: NACA de 4 dígi os
8
4.2.2 Simulación
Pa a los imones de p o undidad y di ección se u iliza el pe il ae odinámico NACA 0012.
Un pe il ae odinámico simé ico que hace de con ol en es e ae omodelo.
Ilus ación 6: Pe il NACA 0012 in oducido en XFLR5
Pa a las alas se ha escogido el pe il ae odinámico Epple E214. Es un pe il
ae odinámico pa a bajos alo es de Reynolds. Pa a pode simula es e pe il
ae odinámico median e el p og ama XFLR5, se ha u ilizado la in o mación disponible
en: h ps://m-selig.ae.illinois.edu/
Ilus ación 7: Pe il Epple E214 in oducido en XFLR5
Pa a con inua , se simulan ambos pe iles ae odinámicos an e di e en es núme os de
Reynolds y ángulos de a aque. Pa a ello se con igu a un conjun o de análisis pa a Reynolds
en e 100.000 y 150.000 a inc emen os de 10.000, y pa a ángulos de a aque en e -2 y 16
g ados con inc emen os de un g ado.
Ilus ación 8: Con igu ación del análisis
15
4.3.1.2 Módulo Mic ocon olado
El módulo Mic ocon olado inco po a un mic ocon olado , los componen es pasi os
necesa ios pa a su uncionamien o y las conexiones al conec o M.2 Es ánda
co espondien es. Cualquie mic ocon olado con la memo ia su icien e, elocidad de eloj
que pe mi a la ejecución del p og ama y sus in e upciones, y buses de comunicación I2C, SPI
y UART puede u iliza se en es e módulo.
ATMEGA2560
Pa a el p o o ipo se ha escogido un mic ocon olado ATMEGA2560 alimen ado a 3.3
Vol ios. És e es un mic ocon olado de 8 bi s que iene como abajo el cálculo de o ien ación
y ayec o ia de la ae ona e a pa i de los senso es disponibles, así como el mando sob e las
supe icies de con ol. En la siguien e imagen se puede obse a un esquemá ico educido de
la alimen ación pa a es e mic ocon olado .
Ilus ación 19: Conexiones de alimen ación del ATMEGA2560

16
4.3.1.3 Módulo de ins umen ación
El módulo de ins umen ación es el módulo donde se encuen an el ecep o GNSS y el
MARG (IMU + Magne óme o). Es e módulo necesi a ecoge del conec o M.2 Es ánda :
alimen ación, bus I2C (pa a la comunicación en e Mic ocon olado y MARG) y bus UART
(pa a la comunicación en e Mic ocon olado y ecep o GNSS).
MPU-9250
El IC MPU-9250 es un conjun o IMU + magne óme o que se ende como módulo pa a
pla a o mas de desa ollo de ha dwa e y es ap oximadamen e 200 eces más ba a o que un
XSens MTi-7 (AHRS que se ha u ilizado como e e encia come cial du an e los ensayos).
Debido a su bajo cos e y su ácil implemen ación en A duino (ya exis en lib e ías que lo
manejan), es e módulo ha sido escogido como unidad MARG de P.A.T.O.
Ilus ación 20: Esquema del módulo MARG MPU9250
Recep o GNSS
El ecep o GNSS de la amilia NEO-6 o ece los da os a a és de UART, a una elocidad de
9.600 bps. Los da os ansmi idos son sen encias de inidas en el p o ocolo NMEA (Na ional
Ma ine Elec onics Associa ion). La in ensidad de co ien e necesa ia es de unos 37 mA en
modo de medición con inuo. Se u iliza un modelo NEO-6M, pues o que se alimen a á a 3.3 V
(También se ía posible u iliza los modelos NEO-6Q que uncionan con la misma ensión).
4.3.1.4 Módulo de Ac uado es
Los se omo o es enca gados de gobe na las supe icies de con ol del ae omodelo
necesi an an o alimen ación como con ol, p oceden e de la a iónica. Se conec an a es e
módulo.
17
4.3.1.5 Módulo de eleme ía
El Módulo de eleme ía se enca ga de man ene una comunicación bidi eccional con la
es ación de ie a.
F ecuencia
A la ho a de escoge la ecuencia de ope ación es impo an e ene en cuen a los e ec os de
di acción, abso ción y e acción. Se pueden sepa a en 4 g upos de p opagación: ondas
e es es, di ec as, de sa éli e e ionos é icas.
Las ondas e es es (Su ace Wa es): Al en ia más allá del ho izon e, en eo ía debe ía
habe somb a (elec omagné ica), las ondas inducen o as co ien es en la ie a haciendo que
és as ap o echen el e ec o de e acción sob e es a nue a zona y que lleguen muy lejos. La
dis ancia depende del ipo de suelo, sob e el que se inducen las co ien es. Las ecuencias de
las ondas e es es se encuen an en e 30 Hz y 3 MHz.
Ilus ación 21: Su ace Wa es
Las ondas di ec as ienen como ca ac e ís ica que se p opagan has a que se encuen an un
obs áculo o el ho izon e. Se u ilizan en odo el espec o elec omagné ico. La dis ancia máxima
de p opagación depende á de la al u a a la que se encuen en las an enas.
√ √
Ondas ionos é icas (Sky Wa es) y de sa éli e (Space Wa es): La a mós e a iene a ias
capas. Du an e los 20 p ime os kilóme os se encuen a la opos e a, después es á la
es a os e a du an e o os 30 kilome os y después es á la ionos e a; es a pa e de la
a mós e a es á si uada en e los 50 y 400 km y es menos densa. La ionos e a hace de capa
eléc ica, al eacciona a la adiación sola , y po lo an o a ec a a las ansmisiones
elec omagné icas.
Desde los 50 km es án la capa D de la ionos e a, la capa E, la capa F1 y la capa F2. De noche,
al no eacciona al sol sólo es á la capa F (F1 + F2. La más ex e na).
18
 Pa a las ondas de UHF (mayo es de 300 MHz) la ionos e a es anspa en e. És as son
las Ondas de sa éli e o Space Wa es.
 Las ecuencias muy bajas son abso bidas po la ionos e a.
 De 3 KHz a 3 MHz las ondas ebo an en la ionos e a ( e lexión). Es impo an e ene en
cuen a que la a iación de las capas en e noche y día hace que la dis ancia de ebo e
cambie, siendo mayo de noche. Cualquie onda e lejada cambia á su pola ización y
po lo an o se e á a enuada.
 Las llamadas Ondas ionos é icas, de ecuencia en e 30 KHz y 30 MHz, e ac an en la
ionos e a.
Ilus ación 22: Esquema con las di e en es ondas
Dado que la ae ona e debe es a siemp e en Línea de Visión, según las no ma i as ac uales,
se escoge una ecuencia en e 3 MHz y 30 GHz pa a el uncionamien o de la eleme ía.
En caso de que apa ezca un obs áculo pun ual (un pája o, un globo…) en e ambas an enas
el adioenlace pod ía e se a ec ado y po an o, se decide que la ecuencia se á
p e e iblemen e baja. Es o se debe a que a ecuencias más bajas, los obs áculos en Línea de
Visión pueden se mi igados g acias al e ec o de la di acción.
19
Modulación GFSK
Se escoge una modulación GFSK pa a hace el abajo.
Teniendo odo lo an e io en cuen a se escoge una ecuencia de abajo 431 MHz. Pa a es e
abajo se u iliza un APC220 cuyo da ashee se encuen a disponible en los anexos. Se u iliza
o o APC220 con un adap ado de UART a USB pa a ecibi la eleme ía median e el moni o
se ie de A duino IDE.
Ilus ación 23: Módulos APC220 + adap ado USB/UART pa a unciona de eleme ía
20
4.3.2 So wa e
El so wa e a desa olla se sepa a p incipalmen e en 2 pa es di e en es. Po un lado es á
el Con ol Guiado de la ae ona e, que pe mi e la ae ona egación median e la o ien ación
espec o a ie a del ae omodelo, y po o o lado es á el Con ol de T ayec o ia, que median e
un sis ema GNSS es capaz de conoce su posición y ayec o ia pa a segui una se ie de pun os
GNSS p e iamen e p og amados. Es os dos lazos se supe ponen, siendo el Con ol de
T ayec o ia el que da las consignas al Con ol Guiado.
Ilus ación 24: Diag ama simpli icado de los lazos de con ol de P.A.T.O.
4.3.2.1 Con ol Guiado
El Con ol Guiado, es el lazo de con ol enca gado de o ien a la ae ona e y po lo an o de
gobe na las supe icies de con ol pa a, po medio de la ins umen ación a bo do, co egi las
eacciones del a ión has a ob ene la o ien ación solici ada.
Na egación aé ea pa a ae omodelos de 2 G.D.L.
Un ae omodelo de 2 g ados de libe ad u iliza el imón de di ección y el imón de
p o undidad pa a con ola su uelo. Más de alles ace ca de la ae ona egación en [2].
Pa a con ola el ángulo de inclinación (pi ch en inglés, no malmen e θ) u iliza el imón de
p o undidad. La acción de es a supe icie de con ol disminuye o aumen a la ue za
descenden e c eada po la pa e ase a del a ión. Una mayo ue za descenden e en la cola,
p oducida po un imón de p o undidad hacia a iba, ue za a la cola del a ión a i hacia abajo
y a la na iz a i hacia a iba y la elocidad se educe. Una disminución de ue za descenden e
en la cola, p oducida po un imón hacia abajo, pe mi e que la cola se ele e y la na iz baje.

21
Pa a con ola la di ección del a ión se u iliza el
imón de di ección. Es impo an e que pa a que es e
con ol uncione el ángulo supe io de las alas debe
se in e io a 180º. Es o es llamado ángulo de died o
y necesi a se posi i o o lo que es lo mismo, un
ángulo in e io a 180º po la pa e de a iba. Un
e ec o adicional del ángulo de died o es la
con ibución a la es abilidad del a ión.
La acción del imón de di ección hace gi a el
ae omodelo en su eje de de i a (yaw en inglés, no malmen e Ψ), lo que hace que exis a un
deslizamien o en e la o ien ación del a ión y la di ección del mismo (y po lo an o de la
di ección de la que eciben el ien o las alas). Es e deslizamien o p o oca (en e o as cosas)
que el ángulo de a aque del ala hacia donde se gi a disminuya mien as que el ángulo de
a aque del ala con a ia aumen e. Es a modi icación de ángulos de a aque hace que la
sus en ación del ala con más ángulo de a aque aumen e p o ocando que el a ión gi e en su eje
de alabeo ( oll en inglés, no malmen e Φ).
Cuando el a ión se encuen a gi ado en su eje de alabeo y la di ección del ien o coincide
con la o ien ación del a ión, el ángulo de a aque en ambas alas es el mismo, po lo que la
ue za de sus en ación es la misma. Dado que el a ión es á gi ado y una de las alas no ealiza
su ue za en e ical (po que es á gi ado) el a ión uel e de o ma na u al a su posición de
eposo (posición es able pa a un uelo ec o y ni elado).
Ilus ación 26: Demos ación del au oni elado median e ángulo de died o
Ilus ación 25: Ángulo de died o
22
Lazo de con ol pa a dos g ados de libe ad
El lazo de con ol pa a dos g ados de libe ad se di ide en 3 bloques p incipales: Con ol
Guiado 2 G.D.L., Plan a y Realimen ación AHRS.
A la ho a de ealiza la plan a del ae omodelo, se plan ea un modelo ma emá ico simpli icado,
donde el ae omodelo es un sólido ígido. És e eacciona a los pa es gene ados po una ue za
cons an e, sob e la supe icie on al apa en e de las supe icies de con ol. Adicionalmen e
es os pa es son some idos a pe u baciones que ep esen an, no sólo pe u baciones du an e
el uelo, sino las au én icas ue zas ae odinámicas a las que el a ión es a á some ido ambién
en un uelo no mal. Es posible hace es o po que se conoce la g an es abilidad de es e
ae omodelo, lo que pe mi e que cualquie i egula idad (como es el modelo excesi amen e
simpli icado que se p opone aquí) se co ija au omá icamen e.
Ilus ación 27: Plan a simpli icada de ae omodelo de 2 G.D.L.
Las supe icies de con ol se mue en g acias a se omo o es; és os se ep esen an como
lazos ce ados (jun o con una sa u ación como limi ación mecánica de posición) al y como se
e en la siguien e ilus ación:
Ilus ación 28: Se omo o es con limi ación mecánica de posición
23
Los se omo o es se encuen an den o del bloque “Con ol guiado 2 G.D.L.”. Es e bloque
ecibe consignas de gi o e inclinación que pos e io men e se adap an. El e o en e es as
consignas y el ángulo Eule co espondien e llega a un PID que gobie na los se omo o es.
Cabe des aca que el alo de la consigna una ez adap ado no puede supe a alo es de
segu idad asignados heu ís icamen e con el in de que el ae omodelo abaje con pa ones de
uelo ya conocidos y que, po lo an o, no en e en pé dida.
Ilus ación 29: Bloque "Con ol Guiado 2 G.D.L."
El con ol sigue los p incipios de na egación aé ea pa a ae ona es de 2 G.D.L.
an e io men e comen ados. Pa a ello, el ascenso se á es ablecido po la inclinación del a ión y
po lo an o el e o en e consigna de ele ación adap ada y la inclinación se á el que gobie ne
el imón de p o undidad. Po o o lado el gi o se á es ablecido po el Roll del a ión y se á el
e o en e és e y la consigna quien de e mine la posición del imón de di ección.
Ilus ación 30: Lazo de con ol 2 G.D.L. simpli icado en Simulink
24
Na egación aé ea pa a ae omodelos de 3 G.D.L.
Un ae omodelo de 3 g ados de libe ad u iliza imón de p o undidad, imón de di ección y
ale ones pa a con ola su uelo. Más de alles ace ca de la ae ona egación en [2].
Pa a con ola el ángulo de inclinación u iliza el imón de p o undidad. La acción de es a
supe icie de con ol disminuye o aumen a la ue za descenden e c eada po la pa e ase a
del a ión, igual que en el ae omodelo de 2 G.D.L.
En ae odinámica, el gi o es una maniob a básica muy compleja e implica el uso de imón de
di ección, imón de p o undidad y ale ones. Du an e el uelo ec o y ni elado, la sus en ación
o al ac úa en e ical y di ec amen e opues a a la g a edad. Al alabea el a ión, la
sus en ación (que sigue siendo pe pendicula al eje ans e sal del ae omodelo) ac úa aho a
en un plano inclinado.
Si se descompone es a sus en ación en dos ec o es (uno e ical y o o ho izon al), el
componen e e ical de la sus en ación se opone a la g a edad. El componen e ho izon al
ac úa como ue za cen ípe a, i ando del a ión hacia el cen o de un eje imagina io. Es a
ue za cen ípe a lo impulsa a gi a al ededo de dicho eje, con ibuyendo la sección de cola a
man ene el ae omodelo alineado con el ien o ela i o en una ayec o ia cu ada.
Ilus ación 31: Fue zas que ac úan en un gi o
Po lo an o, se alabea el a ión pa a inclina la sus en ación. Es a inclinación de la
sus en ación hace que además de sopo a el peso del a ión, se p o ea de ue za cen ípe a,
que man iene al a ión al ededo del eje e ical de gi o, con a es ando la ue za cen í uga
que iende a expulsa al a ión de la ayec o ia cu ada.
31
4.3.5 P og amación del mic ocon olado
A la ho a de p og ama el Mic ocon olado es impo an e ene en cuen a que hay pa e
del p og ama que se ejecu a no malmen e y o a (g an) pa e del p og ama se ejecu a de
o ma cíclica y empo izada. Po lo an o se empieza po el p og ama p incipal. An es de eso se
añaden una se ie de lib e ías:
#include <Wi e.h> //Lib e ía pa a abaja con el bus I2C
#include <ma h.h> //Lib e ía necesa ia pa a el il o de o ien ación
#include <SPI.h> //Lib e ía pa a abaja con el bus SPI
#include <Time One.h> //Lib e ía pa a abaja con in e upciones cíclicas
Con igu ación
Los da os ob enidos del ecep o GNSS NEO-6 es án en o ma o $GPGSV, $GPVGT,
$GPGLL, $GPGSA, $GPRMC y $GPGGA, que son algunas de las secuencias disponibles en el
p o ocolo NMEA. Es as secuencias ienen es a o ma:
$GPGSV,NoMsg,MsgNo,NoS ,{,s ,el ,az,cno}*cs
$GPVTG,cog ,T,cogm,M,sog,N,kph,K,mode*cs
$GPGLL,La i ude,N,Longi ude,E,hhmmss.ss,Valid,Mode*cs
$GPGSA,Smode,FS{,s },PDOP,HDOP,VDOP*cs
$GPRMC,hhmmss.ss,A,llll.ll,a,yyyy.yy,a,x.x,x.x,ddmmyy,x.x,a*cs
$GPGGA,hhmmss.ss,La i ude,N,Longi ude,E,FS,NoSV,HDOP,msl,m,Al e ,m,Di Age,Di S a ion*cs

32
An es de comenza a ob ene los da os, con iene con igu a el módulo GNSS pa a que
ansmi a las amas que con ienen al p oyec o. En es e caso se escogen dos: $GPRMC y
$GPGGA.
//Con igu ación del GNSS
GPS.begin(9600)
while (GPS.a ailable())
GPS. ead();
// Se inhabili a la ama GPGLL
s cpy(linea,”$PUBX,40,GLL,0,0,0,0,0,0*”);
CheckSum();
GPS.p in ln(linea);
// Se inhabili a la ama GPGSA
linea[10] = ‘S’;
linea[11] = ‘A’;
linea[25] = 0;
CheckSum();
GPS.p in ln(linea);
// Se inhabili a la ama GPGSV
linea[11] = ‘V’;
CheckSum();
GPS.p in ln(linea);
// Se inhabili a la ama GPVGT
linea[9] = ‘V’;
linea[10] = ‘G’;
linea[11] = ‘T’;
linea[25] = 0;
CheckSum();
GPS.p in ln(linea);
// Sólo quedan habili adas las secuencias $GPGGA y $GPRMC.
// Selección del pue o UART y su con igu ación
linea[0] = 0xB5; linea[1] = 0x62; // Cabece a UBX
linea[2] = 0x06; linea[3] = 0x00; // Con igu ación de Pue o
linea[4] = 0x14; linea[5] = 0x00; // Longi ud (Se /Ge )
linea[6] = 0x01; // Pue o UART 1
linea[7] = 0x00; // Rese ado
linea[8] = 0x00; linea[9] = 0x00 ; // Tx Ready
linea[10] = 0xD0; linea[11] = 0x08; // Modo UART (1)
linea[12] = 0x00; linea[13] = 0x00; // Modo UART (2)
linea[14] = 0x00; linea[15] = 0x96; // Baud a e 38.400 bps (1)
linea[16] = 0x00; linea[17] = 0x00; // Baud a e 38.400 bps (2)
linea[18] = 0x03; linea[19] = 0x00; // inP o oMask
linea[20] = 0x03; linea[21] = 0x00; // ou P o oMask
linea[22] = 0x00; linea[23] = 0x00; // Rese ado
linea[24] = 0x00; linea[25] = 0x00; // Rese ado
linea[26] = 0x8F; linea[27] = 0x70; // CheckSum
o (i = 0; i < 28; i++) // T ansmi e
GPS.w i e(linea[i]); // Los 28 by es
GPS.end(); // Cie a el Pue o GPS
GPS.begin(38400); // Ab e el pue o GPS a 38.400 bps
33
Pa a con igu a el MPU9250 se puede u iliza la lib e ía o median e egis os. Pa a aho a
memo ia del Mic ocon olado se u ilizan los egis os di ec amen e:
//Se einicia el módulo pa a asegu a un es ado conocido:
Wi e.begin(); // Inicio del I2C
Wi e.beginT ansmission(MPU); // Conexión al MPU
Wi e.w i e(0x6B); // Esc ibe egis o Powe Managemen 1
Wi e.w i e(0); // RESET
Wi e.endT ansmission(TRUE); // Fin de ansmisión
Se empieza po la con igu ación del gi óscopo. El ondo de escala se puede con igu a a 250
dps, 500 dps, 1000 dps o 2000 dps, esc ibiendo en su egis o el alo 0x00, 0x08, 0x10 o 0x18
espec i amen e.
// Con igu ación del Gi óscopo:
Wi e.beginT ansmission(MPU); // Conexión al MPU
Wi e.w i e(0x1B); // Regis o Con ig. Gi oscopo
Wi e.w i e(0x08); // Esc ibe ondo de escala 500 dps pa a gi os.
Wi e.endT ansmission(TRUE); // Fin de la ansmisión
Después se con inúa con la con igu ación del acele óme o. El ondo de escala del
acele óme o se puede ajus a a 2 G, 4 G, 8 G o 16 G, esc ibiendo en su egis o 0x00, 0x08,
0x10 ó 0x18 espec i amen e.
//Con igu ación del acele óme o:
Wi e.beginT ansmission(MPU); // Conexión al MPU
Wi e.w i e(0x1C); // Regis o Con ig. Acele ome o
Wi e.w i e(0x08); // Esc ibe ondo de escala a 4 Gs
Wi e.endT ansmission(TRUE); // Fin de la ansmisión
Po úl imo, el magne óme o no es necesa io con igu a lo, pe o es necesa io accede a él. Es
necesa io que se esc iba el bi “BYPASS_EN” pa a pode ene acceso a él. El magne óme o es
un segundo chip den o del encapsulado del módulo IMU y po lo an o hay que a isa a és e,
que es el dueño del I2C, de que se quie e usa ese disposi i o:
//Acceso al chip in e no magne óme o:
Wi e.beginT ansmission(MPU); // Conexión al MPU
Wi e.w i e(0x37); // Regis o INT Pin/Bypass
Wi e.w i e(2); // Ac i a el bypass (Acceso a AK8963)
Wi e.endT ansmission(TRUE); // Fin de la ansmisión
Se con igu a la in e upción cíclica al pe iodo de mues eo escogido en las simulaciones con
Simulink u ilizando la lib e ía Time One.h:
Time 1.a achin e up (ejecucion_ciclica);
Time 1.ini ialize(PERIODO_MUESTREO_US); //Valo en us
34
Con es o se e mina el se up y se comienza con el loop del p og ama:
Pa a in e p e a la secuencia se puede p og ama un pa se que sepa e los alo es que
sean de in e és. Pa a ello se p esen a un código que ap o echa las pa icula idades de ASCIIZ
pa a hace más ácil el abajo. Pa a es o se buscan los sepa ado es (comas) y se sus i uyen
po el ca ác e de con ol null (0x00), a la ez que se esc ibe en cada posición del a ay de
pun e os “da o” la di ección de inicio de cada uno de ellos, has a encon a el as e isco, que
indica la llegada del checksum. Con es o cada pun e o apun a a un da o de la sen encia leída.
i = 5;
j = 0;
da o[j ++] = &(linea[3])];
while (linea[i]) != ‘*’) {
i (linea[i] == ‘,’) {
da o[j++] = &(linea[i + 1]);
linea[i] = 0;
}
i++;
i (i > 89)
b eak;
}
Aho a se comp ueba si la ama es $GPRMC o &GPGGA. En caso de se &GPRMC se ob iene
el alo de la di ección del ec o de ayec o ia. Si la ama es &GPGGA, se ecoge la posición
ac ual (la i ud, longi ud y al u a). Todo se gua da en un a ay de nomb e “ egis o”.
i (j > 8) {
i (!s cmp(da o[0], ‘GGA’)) {
// Sen encia GGA **********************************************************
s cpy( egis o[HORA], da o[1]);
i (da o[6][0] != ‘0’) {
s cpy ( egis o[LATITUD_N], da o[2]);
s cpy ( egis o[LATITUD_A], da o[3]);
s cpy ( egis o[LONGITUD_N], da o[4]);
s cpy ( egis o[LONGITUD_A], da o[5]);
s cpy ( egis o[ALTITUD], da o[9]);
}
}
else i (!s cmp(da o[0], ‘RMC’)) {
//Sen encia RMC **********************************************************
s cpy( egis o[HORA], da o[1]);
i (da o[6][0] != ‘0’) {
s cpy ( egis o[CURSO], da o[7]);
}
}
}
35
Aho a, pa a e i a que la in e upción coja los da os a medias se u iliza un lag y un a ay
llamado egis o_copia en el que olca los da os. De es e modo se e i a que cuando se es é
esc ibiendo en uno de ellos, los da os ecogidos po el algo i mo cíclico puedan se e óneos.
lag_ olcado = 0;
s cpy ( egis o_copia[LATITUD_N], egis o[LATITUD_N]);
s cpy ( egis o_copia[LATITUD_A], egis o[LATITUD_A]);
s cpy ( egis o_copia[LONGITUD_N], egis o[LONGITUD_N]);
s cpy ( egis o_copia[LONGITUD_N], egis o[LONGITUD_N]);
s cpy ( egis o_copia[ALTITUD], egis o[ALTITUD]);
lag_ olcado = 1;
Con es o se acaba la unción loop. Sólo quedan unciones de usua io. La p ime a de ellas es
la unción CheckSum la cual se enca ga de calcula el CheckSum de los mensajes de
con igu ación del ecep o GNSS.
oid CheckSum() {
cs = 0;
o (posicion = 1; linea[posicion] != ‘*’; posicion++) // XOR con cs pa a cada by e
cs = cs ^ línea[posición];
l = cs % 16; // Hexadecimal bajo
h = (cs – l) / 16; // Hexadecimal al o
l += 48; // Con e sión a cha
i (l > 57) // Si es mayo a 9
l += 7; // Esc ibe le a (A-F)
h += 48; // Con e sión a cha
i (h > 57) // Si es mayo a 9
h += 7; // Esc ibe le a (A-F)
linea[++ posición] = h; // Se esc ibe el CheckSum
linea[++ posición] = l;
linea[++ posición] = 0;
}
Después a la unción del il o de o ien ación:
A la ho a de implemen a es e algo i mo con A duino IDE, hace al a inclui la lib e ía
ma h.h. Se empieza po c ea una unción que u ilice los alo es ecogidos po el MARG pa a su
uncionamien o:
oid madgwick_ac ualiza MARG( loa gx, loa gy, loa gz,
loa ax, loa ay, loa az,
loa mx, loa my, loa mz) {
Den o de la unción se empieza po decla a odas las a iables que se u iliza án. Todas las
a iables que se decla an son ipo loa .
Se ob iene el alo de la de i ada del cua e nión (el cua e nión es á o mado po a iables
globales).
qDo 1 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz);
qDo 2 = 0.5 * ( q0 * gx + q2 * gz - q3 * gy);
qDo 3 = 0.5 * ( q0 * gy - q1 * gz + q3 * gx);
qDo 4 = 0.5 * ( q0 * gz + q1 * gy - q2 * gx);
36
Lo siguien e se calcula sólo en caso de que el alo ecibido del acele óme o sea co ec o,
po lo an o se u iliza una condición:
i (!((ax == 0.0 ) && (ay == 0.0 ) && (az == 0.0 )))
P ime o se no malizan las medidas de acele óme o y magne óme o:
//No malización del acele óme o
ecipNo m = in Sq (ax * ax + ay * ay + az * az);
ax *= ecipNo m;
ay *= ecipNo m;
az *= ecipNo m;
// No malización del magne óme o
ecipNo m = in Sq (mx * mx + my * my + mz * mz);
mx *= ecipNo m;
my *= ecipNo m;
mz *= ecipNo m;
Aho a, pa a e i a calcula epe idamen e, se calculan una se ie de a iables auxilia es.
_2q0mx = 2.0 * q0 * mx;
_2q0my = 2.0 * q0 * my;
_2q0mz = 2.0 * q0 * mz;
_2q1mx = 2.0 * q1 * mx;
_2q0 = 2.0 * q0;
_2q1 = 2.0 * q1;
_2q2 = 2.0 * q2;
_2q3 = 2.0 * q3;
_2q0q2 = 2.0 * q0 * q2;
_2q2q3 = 2.0 * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
Con las a iables auxilia es ya calculadas, se empieza po calcula la e e encia e es e
median e el campo magné ico:
hx = mx*q0q0 - _2q0my*q3 + _2q0mz*q2 + mx*q1q1 + _2q1*my*q2 + _2q1*mz*q3 - mx*q2q2 - mx*q3q3;
hy = _2q0mx*q3 + my*q0q0 - _2q0mz*q1 + _2q1mx*q2 - my*q1q1 + my*q2q2 + _2q2*mz*q3 - my*q3q3;
_2bx = sq (hx * hx + hy * hy);
_2bz = -_2q0mx*q2 + _2q0my*q1 + mz*q0q0 + _2q1mx*q3 - mz*q1q1 + _2q2*my*q3 - mz*q2q2 + mz*q3q3;
_4bx = 2.0 * _2bx;
_4bz = 2.0 * _2bz;

37
Se con inúa con el algo i mo g adien e descenden e y su no malización:
// Algo i mo co ec i o de g adien e descenden e
s0 = -_2q2 * (2.0 * q1q3 - _2q0q2 - ax) + _2q1 * (2.0 * q0q1 + _2q2q3 - ay)
- _2bz * q2 * (_2bx * (0.5 - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx)
+ (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my)
+ _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5 - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0 * q1q3 - _2q0q2 - ax) + _2q0 * (2.0 * q0q1 + _2q2q3 - ay)
- 4.0 * q1 * (1 - 2.0 * q1q1 - 2.0 * q2q2 - az)
+ _2bz * q3 * (_2bx * (0.5 - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx)
+ (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my)
+ (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5 - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0 * q1q3 - _2q0q2 - ax) + _2q3 * (2.0 * q0q1 + _2q2q3 - ay)
- 4.0 * q2 * (1 - 2.0 * q1q1 - 2.0 * q2q2 - az)
+ (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5 - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx)
+ (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my)
+ (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5 - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0 * q1q3 - _2q0q2 - ax) + _2q2 * (2.0 * q0q1 + _2q2q3 - ay)
+ (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5 - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx)
+ (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my)
+ _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5 - q1q1 - q2q2) - mz);
ecipNo m = in Sq (s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // No maliza magni ud
s0 *= ecipNo m;
s1 *= ecipNo m;
s2 *= ecipNo m;
s3 *= ecipNo m;
Finalmen e se aplica la ealimen ación:
// Aplica ealimen ación
qDo 1 -= be a * s0;
qDo 2 -= be a * s1;
qDo 3 -= be a * s2;
qDo 4 -= be a * s3;
Aho a se sale de la condición de los acele óme os (po lo an o, lo que se calcule a pa i de
aho a se ejecu a á sean co ec os los alo es o no). Se incluye la de i ada del cua e nión al
cua e nión:
q0 += qDo 1 * in SampleF eq;
q1 += qDo 2 * in SampleF eq;
q2 += qDo 3 * in SampleF eq;
q3 += qDo 4 * in SampleF eq;
Y inalmen e se no maliza el cua e nión:
ecipNo m = in Sq (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= ecipNo m;
q1 *= ecipNo m;
q2 *= ecipNo m;
q3 *= ecipNo m;
anglesCompu ed = 0;
38
Con es o las a iables q0, q1, q2 y q3 ienen el alo del cua e nión de o ien ación calculado.
Pa a que es a unción pueda se calculada se u iliza la unción in Sq . En las siguien es líneas
de código puede e se la unción in Sq :
loa in Sq ( loa x) {
loa hal x = 0.5 * x;
loa y = x;
long i = *(long *)&y;
i = 0x5 3759d - (i>>1);
y = *( loa *)&i;
y = y * (1.5 - (hal x * y * y));
y = y * (1.5 - (hal x * y * y));
e u n y;
}
Adicionalmen e, se u iliza án los ángulos de Eule du an e los expe imen os, ya que
esul an más sencillos de en ende , po lo an o la siguien e unción se á la enca gada de
ealiza el cambio de cua e niones a ángulos de Eule :
oid compu eAngles() {
oll = a an2 (q0*q1 + q2*q3, 0.5 - q1*q1 - q2*q2);
pi ch = asin (-2.0 * (q1*q3 - q0*q2));
yaw = a an2 (q1*q2 + q0*q3, 0.5 - q2*q2 - q3*q3);
anglesCompu ed = 1;
}
La siguien e es la unción que se ejecu a á cíclicamen e, p ime o se ecogen los da os del
MPU 9250, empezando po los acele óme os. Hab á que ealiza un ajus e en unción de la
sensibilidad escogida en el acele óme o.
oid ejecucion_ciclica() {
//Acele óme os:
Wi e.beginT ansmission(MPU);
Wi e.w i e(0x3B); //Solici a acele óm e os
Wi e.endT ansmission(FALSE);
Wi e. eques F om(MPU, 6, ue);
// Acele óme o OX:
lec u a = (Wi e. ead() <<8 | Wi e. ead()) / ajus e[accel];
ac2al a();
s cpy( egis o[ACCEL_X], in e );
// Acele óme o OY:
lec u a = (Wi e. ead() <<8 | Wi e. ead()) / ajus e[accel];
ac2al a();
s cpy( egis o[ACCEL_Y], in e );
// Acele óme o OZ:
lec u a = (Wi e. ead() <<8 | Wi e. ead()) / ajus e[accel];
ac2al a();
s cpy( egis o[ACCEL_Z], in e );
39
Se epi e el mismo p ocedimien o pa a los gi óscopos:
// Gi óscopos:
Wi e.beginT ansmission(MPU);
Wi e.w i e(0x43); //Solici a gi óscopos
Wi e.endT ansmission(FALSE);
Wi e. eques F om(MPU, 6, ue);
// Gi óscopo OX:
lec u a = (Wi e. ead() <<8 | Wi e. ead()) / ajus e[accel];
ac2al a();
s cpy( egis o[GYRO_X], in e );
// Gi óscopo OY:
lec u a = (Wi e. ead() <<8 | Wi e. ead()) / ajus e[accel];
ac2al a();
s cpy( egis o[GYRO_Y], in e );
// Gi óscopo OZ:
lec u a = (Wi e. ead() <<8 | Wi e. ead()) / ajus e[accel];
ac2al a();
s cpy( egis o[GYRO_Z], in e );
Pa a los magne óme os, as pedi acceso al chip del magne óme o se lee el egis o 0x02
has a que con enga el bi 1 ac i o, en es e momen o se puede accede a los egis os que
con ienen los alo es de los magne óme os.
Wi e.beginT ansmission(MAG);
Wi e.w i e(0x0A);
Wi e.w i e(0x11);
Wi e.endT ansmission( ue);
da a_ eady=0;
while (! (da a_ eady & 1)) {
Wi e.beginT ansmission(MAG);
Wi e.w i e(0x02);
Wi e.endT ansmisission( alse);
Wi e. eques F om(MAG, 1, TRUE);
da a_ eady = (Wi e. ead())
}
Wi e.beginT ansmission(MAG);
Wi e.w i e(0x03);
Wi e.endT ansmission( alse);
Wi e. eques F om(MAG, 6, ue);
i oa((Wi e. ead() | Wi e. ead() <<8), sbu e , 10);
s cpy( egis o[MAG_X], sbu e );
i oa((Wi e. ead() | Wi e. ead() <<8), sbu e , 10);
s cpy( egis o[MAG_Y], sbu e );
i oa((Wi e. ead() | Wi e. ead() <<8), sbu e , 10);
s cpy( egis o[MAG_Z], sbu e );
Una ez ob enidos los da os se llama a la unción del il o de o ien ación, a la se da án los
da os pa a calcula la o ien ación. Se le solici a ambién que haga la con e sión a ángulos de
Eule al inal:
//Calcula o ien ación:
madgwick_ac ualiza MARG( egis o[GYRO_X], egis o[GYRO_Y], egis o[GYRO_Z],
egis o[ACCEL_X], egis o[ACCEL_Y], egis o[ACCEL_Z],
egis o[MAG_X], egis o[MAG_Y], egis o[MAG_Z]);
Con e sión a ángulos de Eule :
madgwick_compu eAngles();
40
Las a iables con los ángulos de Eule son a iables globales, po lo que el es o de
unciones pueden accede a ellas.
Después se p ocede a p og ama los PIDs necesa ios pa a el con ol. Pa a p og ama PIDs
en mic ocon olado es se sigue el a ículo “Cómo implemen a un PID con
Mic ocon olado es” incluido en los anexos. El PID se ejecu a en una in e upción pe iódica,
que asegu a que la ecuencia de mues eo es siemp e la misma.
Po lo an o, pa a el PID, en el bucle p incipal no hay nada. Todo se encuen a den o de la
p opia in e upción (a excepción de la decla ación de a iables). En caso de sa u a la salida,
es e PID puede da p oblemas, po lo que se le incluye un an i wind-up pa a e i a es e
p oblema. Se empieza po el PID que gobie na el imón de p o undidad:
// Cálculo de con olado :
unsigned long now_pi ch = millis();
in imeChange_pi ch = (now_pi ch – las Time_pi ch);
double e o _pi ch = al u a_consigna_pi ch - pi ch;
ITe m_pi ch += (ki_pi ch * e o _pi ch);
I (ITe m_pi ch > ou Max)
ITe m_pi ch = ou Max;
else i (ITe m_pi ch < ou Min)
ITe m_pi ch = ou Min;
double dInpu _pi ch = (pi ch – las Inpu _pi ch);
// Calcula salida del PID:
Ou pu _pi ch = kp_pi ch * e o _pi ch + ITe m_pi ch – kd_pi ch * dInpu _pi ch;
//Se sa u a la salida pa a que el núme o no siga c eciendo
I (Ou pu _pi ch > ou Max)
Ou pu _pi ch = ou Max;
else i (Ou pu _pi ch < ou Min)
Ou pu _pi ch = ou Min;
/*Memo ia pa a el p óximo ciclo*/
las Inpu _pi ch = pi ch;
las Time_pi ch = now_pi ch;
// esc ibi la salida del con olado
analogW i e(TIMON_PROFUNDIDAD, ou pu );
47
Po un lado, si da iempo a en ega odo al bu e , la ansmisión segui á sin in e ención
del mic ocon olado , al y como se e en la ilus ación an e io . En caso con a io, hab á un
e a do, en e dos de los by es ansmi idos po adio, de 15,7 ms. En cualquie caso la
comunicación no se e me mada y los iempos se cumplen.
Ilus ación 47: Rep esen ación del iempo de los subp ocesos con in e upción in e media
020 40 60 80 100
Radio
GNSS
P ocesado
MARG

48
5.3 P ueba con ae omodelo
Debido a p oblemas con la imp eso a 3D, las p ime as p uebas de uelo se ealizan con un
ae omodelo come cial (Una éplica del Pipe J-3 Cub conc e amen e).
Ilus ación 48: Pipe J-3 Cub
Tan o las p uebas de uelo ec o y ni elado como de gi o son sa is ac o ias y los algo i mos
de con ol hacen co ec amen e su abajo. Pese a es o, se obse a un mo imien o ex año a la
ho a de ola con ien os ue es.
Cuando no hay mucho ien o, el ae omodelo se di ige di ec amen e hacia el pun o de
des ino, al y como cabe espe a .
Ilus ación 49: Vuelo sin ien o
49
Cuando un ien o cons an e y ue e golpea al ae omodelo, és e empieza a ac ua a o:
comienza a gi a en di ección al ien o pa a después empeza a hace zig-zag. Las conclusiones
de los expe imen os son las siguien es.
P ime o hay que ene en cuen a el escena io, el a ión a di ec amen e al pun o ma cado y
ecibe ien o ue e desde un cos ado.
Ilus ación 50: A ión en uelo ecibe ien o ue e
Pa a compensa el cambio de ayec o ia, és e gi a con a el ien o.
Ilus ación 51: Compensación de la ayec o ia
50
Si el ien o es lo su icien emen e ue e, el a ión gi a á más aún. Es e gi o excesi o
desemboca en que el algo i mo de con ol de ayec o ia solici e un gi o en sen ido con a io.
Cuando es e segundo gi o se excede nue amen e el sen ido de gi o uel e a cambia .
Ilus ación 52: Reacción del a ión olando con a ue es ien os
La azón de es e compo amien o se debe a que la elocidad del ien o es mayo a la
elocidad de mo imien o (con espec o al ai e) del ae omodelo. Es e hecho hace que la
suposición de que la di e encia en e el ec o de ayec o ia y el de o ien ación sea
desp eciable no se cumpla, debido a que el ec o de ayec o ia bien pod ía ene una
di e encia de 180º con la o ien ación de la ae ona e en es e caso.
51
Cuando el ae omodelo uela con a el ien o ue e, decide gi a en una de las dos
di ecciones.
Ilus ación 53: Compo amien o olando en e al ien o
Es e gi o p o oca un cambio en la ayec o ia.
Ilus ación 54: Cambio del ec o de ayec o ia
Es e cambio en la ayec o ia a ec a al lazo de con ol de ayec o ia pues o que aho a es,
según el algo i mo que se ha p og amado, más e icien e gi a en la o a di ección debido a que
el ángulo es meno en e el ec o de ayec o ia y el de des ino. El cambio de gi o con inúa
has a que el ien o ue e desapa ece.
Ilus ación 55: Cambio del sen ido de gi o
52
6 Conclusiones
El sis ema P.A.T.O. es un sis ema de bajo cos e capaz de ealiza an o uelos de búsqueda
como uelos expe imen ales pa a la ca ga en la bahía (del ae omodelo de ala ija que con ola)
de o ma au ónoma y sin necesidad de la in e ención de un ope ado (aunque es necesa ia su
supe isión po mo i os de segu idad). Pe o como odo sis ema de bajo cos e, és e iene sus
limi aciones.
6.1 Au onomía
En lo e e en e al “alcance y au onomía” del ehículo escogido pa a P.A.T.O., no dispone de
demasiada au onomía debido a la poca ene gía que es capaz de ca ga (en pa e se debe a se
eléc ica su p opulsión).
6.1.1 Tu bo an o Tu boje
Como p opues a de mejo a se p opone el diseño y ab icación de un mo o u boje o un
u bo an. Es o p esen a ía nue os p oblemas debido al mo imien o del combus ible den o del
anque y al cambio de la masa o al a lo la go del uelo, así como el mo imien o del cen o de
masas a lo la go del mismo.
Ilus ación 56: Esquema básico de un u boje
6.1.2 Paneles sola es
O a opción se ía inco po a paneles sola es en la pa e supe io de la supe icie del
ae omodelo. Se sugie e pone los en las alas y, median e un con e ido DC-DC, eca ga las
ba e ías (p incipalmen e las ba e ías de la p opulsión, pe o la a iónica debe eca ga se
ambién, así que es o desemboca en un diseño de BMS o Ba e y Managemen Sys em muy
in e esan e). Como des en aja es a solución sólo puede u iliza se du an e el día y pa a
ope aciones noc u nas únicamen e añade peso al ae omodelo.

53
6.2 Ae omodelo
El ae omodelo escogido pa a lle a el p o o ipo P.A.T.O. es pe ec amen e capaz de lle a a
cabo las a eas que se le han asignado: econocimien o, búsqueda de obje i os, uelo
p ediseñado pa a expe imen os…
6.2.1 Es abilidad
Es o se debe en g an pa e a que las espues as del ae omodelo son len as, la elocidad no
es muy al a y que el p opio diseño del modelo se puede ca aloga como es able. Es a
es abilidad implica que el a ión iende a man ene un uelo ec o y ni elado po sí solo. Es
simila a ene una canica sob e una supe icie en o ma de “U”: po mucho que la canica se
mue a hacia uno de los ex emos, en el ins an e que deje de in e eni la ue za que la ha
mo ido, ella misma ol e á al cen o de o ma na u al; ambién iene un cie o cos e
ene gé ico mo e la canica y es una buena o ma de ilus a la len i ud del sis ema. Un ejemplo
de un ae omodelo es able se ía una a ione a.
Se ía in e esan e desa olla una siguien e e sión del sis ema P.A.T.O. con un ae omodelo
más c í icamen e es able y que po lo an o sea más ápido en eacciones. Un símil del sis ema
c í icamen e es able se ía una canica en una llanu a. No equie e mucha ene gía mo e la
canica (po lo an o elocidad de eacción más al a) y és a no ol e ía al cen o po sí sola. Un
ejemplo se ía un caza, como el F-22.
No se aconseja el uso de P.A.T.O. en un ae omodelo ines able. És e end ía eacciones
excesi amen e ápidas y p esen a ía muchos p oblemas pa a an sólo man ene un uelo ec o
y ni elado. Un símil se ía una canica en una “U” in e ida. No hay cos o ene gé ico al desplaza
la canica ue a del cen o, pe o cos a ía ol e a él (y no ol e ía sola, más bien lo con a io).
Como ilus ación de es e caso, la NASA necesi ó 3 o denado es en pa alelo pa a ealiza las
co ecciones necesa ias pa a el uelo ec o y ni elado en su X-29, cosa que jus i ica es e
consejo sob adamen e.
6.2.2 Sus en ación, en ada en pé dida e hipe sus en ado es
El coe icien e de sus en ación máximo de e mina la ca ga ala del a ión. Po lo an o,
desa olla disposi i os hipe sus en ado es (Flaps y Sla s) pa a aumen a el alo del
coe icien e de sus en ación máximo se ía una ampliación in e esan e. Más de alles ace ca del
ema en [1].
6.2.3 Velocidad
La ae ona e que P.A.T.O. gobie na es á diseñada pa a uelos len os de ca a a una mejo
obse ación du an e las ope aciones de econocimien o y búsqueda. Puede se in e esan e
ediseña la ae ona e pa a pe mi i uelos más ápidos con la in ención de amplia las
posibilidades de los uelos con ines expe imen ales (expe imen os en la bahía de ca ga), así
como su alcance.
54
6.3 Posicionamien o global
En p incipio P.A.T.O. dispone de un sis ema G.N.S.S. conec ado a la ed GPS no eame icana
pa a conoce su ubicación en el globo (así como la al u a).
6.3.1 P ecisión
La p ecisión de una sola ed de sa éli es G.N.S.S. es su icien e pa a la ae ona egación, pe o
puede se insu icien e pa a un buen a e izaje. Pa a mejo a la p ecisión se pueden usa 2
edes de sa éli es simul áneamen e (con 2 ecep o es G.N.S.S.) a cos a de un mayo consumo
eléc ico.
6.3.2 Pé dida de la señal
Cuando las an enas de base se encuen an en man enimien o, la señal G.N.S.S. pe de á
p ecisión. Es o es ine i able, pe o gene almen e se iene cobe u a de has a 3 an enas de base,
po lo que no a ec a ía. És as si en pa a co egi e o es y consegui una mayo p ecisión.
Ilus ación 57: An ena base GNSS
En caso de o men a eléc ica es muy p obable que la o men a p oduzca in e e encias y
po lo an o se pie da señal G.N.S.S. Es una de las p incipales azones po las que no es á
pe mi ido usa P.A.T.O. cuando exis a la posibilidad de o men a eléc ica.
Pese a es o, un ediseño del conjun o an ena + ampli icado , del ecep o GNSS puede se
in e esan e como mejo a.
55
6.4 Algo i mos de con ol
Los algo i mos de con ol de la ae ona e cumplen su unción según lo es ipulado. Pese a
ello, es posible mejo a ambos algo i mos de con ol.
6.4.1 Con ol Guiado
El con ol guiado desc i o en es a memo ia ealiza los gi os al y como se explica en los
manuales de ae ona egación ( e [2] pa a más de alle). Las maniob as de ascenso y descenso,
po el con a io, no siguen es as no mas al pie de la le a; se ía in e esan e modi ica el
algo i mo de con ol guiado pa a que haga es as maniob as según el manual de
ae ona egación.
6.4.2 Con ol de ayec o ia
El con ol de ayec o ia desc i o aquí supone que la di e encia en e el ec o de
ayec o ia y el de o ien ación son desp eciables. Pa a la mayo ía de ayec os es e con ol es
adecuado, pe o, en caso de que enga ien o de en e a una elocidad mayo que su elocidad
ae odinámica, el ec o de ayec o ia apun a ía en di ección opues a.
Dado el diseño del con ol, P.A.T.O. in en a ía co egi su ayec o ia gi ando a un lado. Es e
gi o modi ica ía el ec o de ayec o ia lo su icien e como pa a que el lazo de con ol aplique
un gi o al o o lado. Es o p o oca ía un mo imien o en zig-zag en di ección al des ino
escogido.
Haciendo que la maniob a sea p og esi a es e zig-zag se e ía minimizado, debido al
con inuo cambio de sen ido. Modi ica el con ol de ayec o ia de es a mane a, supond ía un
aumen o de e iciencia pa a el p oyec o.
6.4.3 Modo Repe ido
Cuando P.A.T.O. ecibe la señal de adio del obje i o que es á buscando, sigue su
ayec o ia. Añadi una uncionalidad llamada Modo Repe ido es un paso lógico pa a es e
p oyec o.
En el momen o en el que la bahía de ca ga (equipada con un adio epe ido pa a el
obje i o a busca ) ecibe la señal de baliza del obje i o, pod ían hace se dos cosas:
 La posición GNSS ac ual es almacenada como una a iable nue a y la máquina de
es ados cambia a un es ado nue o que incluye es a posición. Es o implica que el
ae omodelo gi a á en cí culos in en ando i al pun o una y o a ez. T as ecibi el
ACK desde base, se con inúa con la máquina de es ados o iginal (lis a o iginal de
pun os GNSS).
 El adio epe ido ecibe y desenc ip a la posición del obje i o. La posición
desenc ip ada se en ega a P.A.T.O., que c ea una nue a a iable con es a nue a
posición y epi e el p oceso desc i o en el pá a o an e io .
56
Es di ícil sabe cuál de los dos mé odos es mejo . En el caso de ecibi un ebo e de la señal
momen áneamen e, da uel as al ededo del pun o ac ual no end ía mucho sen ido; en el
caso de que el obje i o es é cubie o po algo me álico (un ejado me álico po ejemplo),
posiciona se sob e él lle a ía al adio epe ido a un pun o de somb a. El desa ollo de es e
modo de uncionamien o, así como la elación de comp omiso en e los dos mé odos (o incluso
algún mé odo al e na i o) se án obje o de la p óxima i e ación de P.A.T.O.
6.5 Lazo de con ol de elocidad ae odinámica
El con ol de mo o ac ualmen e en es e ae omodelo consis e en un “ odo o nada”. Añadi
un con ol de elocidad que sea capaz de man ene la elocidad ae odinámica del apa a o, e
incluso de asegu a una elocidad ae odinámica mínima pa a man ene la sus en ación del
a ión, es algo a ene en cuen a como ampliación del sis ema.
Como ins umen o de medición de la elocidad ae odinámica se u iliza ía un ubo de Pi o y
como mo o se u iliza ía un mo o sínc ono o un b ushless (siendo es e segundo el más lógico
po su bajo cos e). Pa a gobe na el mo o se p opone el in eg ado DRV8343-Q1 de Texas
Ins umen s, que pe mi e incluso el con ol de mo o es b ushless con senso es Hall.
Ilus ación 58: Módulo A duino compa ible BMP280 (Senso Ba omé ico)
Pa a el ubo de Pi o se p opone el uso de los senso es ba omé ico BMP280. Es posible
ealiza un pequeño mon aje en el que se midan an o la p esión o al (con uno de los
senso es) como la p esión es á ica (con o o). Teniendo ambas, se puede ob ene la p esión
dinámica y desde ahí calcula la elocidad ae odinámica del a ión. En caso de que e
inco po a el senso a una PCB p opia el siguien e esquema puede se i como e e encia.
Ilus ación 59: Esquemá ico BME280 (Chip de BMP280)
63
G upo
Señal
I/O
Desc ipción
Tensión
CAN
CAN_RX
I
CAN Bus ecei e
da a.
3.3 V
CAN_TX
O
CAN Bus ansmi
da a.
3.3 V
UART
UART_RX1
I
UART ecei e
da a
3.3 V
UART_TX1
O
UART ansmi
da a
3.3 V
UART_RTS1
O
UART eady o
send
3.3 V
UART_CTS1
I
UART clea o
send
3.3 V
UART_RX2
I
2nd UART ecei e
da a
3.3 V
UART_TX2
O
2nd UART
ansmi da a
3.3 V

64
G upo
Señal
I/O
Desc ipción
Tensión
I2C
I2C_SCL
I/O
I2C clock. Open
d ain wi h
pullup on ca ie
boa d
3.3 V
I2C_SDA
I/O
I2C da a. Open
d ain wi h
pullup on ca ie
boa d.
3.3 V
I2C_INT#
I
In e up
no i ica ion om
ca ie boa d o
p ocesso . Open
d ain wi h
pullup on ca ie
boa d. Ac i e
LOW.
3.3 V
I2C_SCL1
I/O
2nd I2C clock.
Open d ain wi h
pullup on ca ie
boa d.
3.3 V
I”C_SDA1
I/O
2nd I2C da a.
Open d ain wi h
pullup on ca ie
boa d.
3.3 V
65
G upo
Señal
I/O
Desc ipción
Tensión
SPI
SPI_COPI
O
SPI Con olle
Ou pu /Pe iphe al
Inpu
3.3 V
SPI_CIPO(I)
I
SPI Con olle
Inpu /Pe iphe al
Ou pu
3.3 V
SPI_SCK
O
SPI Clock
3.3 V
SPI_CS#
O
SPI Chip Selec .
Ac i e LOW. Can
be ou ed o GPIO
i ha dwa e CS is
unused.
3.3 V
SPI/SDIO
SPI_SCK1/SDIO_CLK
O
2nd SPI Cloc.
Seconda y use is
SDIO Clock
3.3 V
SPI_COPI1/SDIO_CMD
I/O
2nd SPI Con olle
Ou pu /Pe iphe al
Inpu . Seconda y
use is SDIO
command
in e ace.
3.3 V
SPI_CIPO1/SDIO_DATA0
I/O
2nd SPI
Pe iphe al
Inpu /Con olle
Ou pu . Seconda y
use is SDIO da a
Exchange bi 0.
3.3 V
SDIO_DATA1
I/O
SDIO da a
Exchange bi 0.
3.3 V
SDIO_DATA2
I/O
SDIO da a
Exchange bi 0.
3.3 V
SPI_CS1/SDIO_DATA3
I/O
2nd SPI Chip
Selec . Seconda y
use is SDIO da a
Exchange bi 0.
3.3 V
66
G upo
Señal
I/O
Desc ipción
Tensión
Audio
AUD_MCLK
O
Audio mas e
clock
3.3 V
AUD_OUT/PCM_OUT/I2S_OUT/CAM_MCLK
O
Audio da a
ou pu . PCM
synch onus da a
ou pu . I2S se ial
da a ou . Came a
mas e clock
3.3 V
AUD_IN/PCM_IN/I2S_IN/CAM_PCLK
I
Audio da a
inpu . PCM
synch onus da a
inpu . I2S se ial
da a in. Came a
pe iphe al clock
3.3 V
AUD_LRCLK/PCM_SYNC/I2S_WS/PDM_DATA
I/O
Audio le / igh
clock. PCM
Synch onous
da a SYN. I2S
Wo d selec .
PDM da a.
3.3 V
AUD_BCLK/PCM_CLK/I2S_CLK/PDM_CLK
O
Audio bi clock.
PCM clock. I2S
con inous se ial
clock. PDM clock.
3.3 V
SWD
SWDIO
I/O
Se ial Wi e
Debug I/O.
Connec i
p ocesso boa d
suppo s SWD.
Can be le NC.
3.3 V
SWDCK
I
Se ial Wi e
Debug clock.
Connec i
p ocesso boa d
suppo s SWD.
Can be le NC.
3.3 V
67
G upo
Señal
I/O
Desc ipción
Tensión
ADC
A0
I
Analog o digi ial
con e e 0.
Ampli y he
analog signal as
needed o enable
ull 0-3.3V ange.
3.3 V
A1
I
Analog o digi al
con e e 1.
Ampli y he
analog signal o
enable ull 0-3.3V
ange.
3.3 V
PWM
PWM0
O
Pulse Wid h
Modula ed
Ou pu 0
3.3 V
PWM1
O
Pulse Wid h
Modula ed
Ou pu 1
3.3 V
Digi al
D0
I/O
Gene al digi al
inpu /ou pu pin.
3.3 V
D1/CAM_TRIG
I/O
Gene al digi al
inpu /ou pu pin.
Came a T igge .
3.3 V
68
G upo
Señal
I/O
Desc ipción
Tensión
Gene al/Bus
G0/BUS0
I/O
Gene al pu pose
pins. Any unusd
p ocesso pins
should be
assigned o Gx
wi h ADC + PWM
capable pins gi en
p io i y (0, 1, 2,
e c.) posi ions.
The in en is o
gua an ee PWM
ADC and Digi al
Pin unc ionali y
on espec i e
ADC/PWM/Digi al
pins. Gx pins do
no gua an ee
ADC/PWM
unc ion.
Al e na i use is
pins can suppo a
as ead/w i e 8-
bi o 4-bi wide
bus.
3.3 V
G1/BUS1
I/O
3.3 V
G2/BUS2
I/O
3.3 V
G3/BUS3
I/O
3.3 V
G4/BUS4
I/O
3.3 V
G5/BUS5
I/O
3.3 V
G6/BUS6
I/O
3.3 V
G7/BUS7
I/O
3.3 V
G8
I/O
Gene al pu pose
pin
3.3 V
G9/ADC_D-/CAM_HSYNC
I/O
Di e en ial ADC
inpu i a ailable.
Came a ho izon al
syn
3.3 V
G10/ADC_D+/CAM_VSYNC
I/O
Fie e en ial ADC
inpu i a ailable.
Came a e ical
sync.
3.3 V
G11/SWO
I/O
Gene al pu pose
pin. Se ial W i e
Ou pu
3.3 V

69
8.2 Fil o de o ien ación
8.2.1 Rep esen ación po “Cua e niones”
La siguien e in o mación se ha ob enido de [7]. Un cua e nión es un núme o complejo de
cua o dimensiones que puede u iliza se pa a ep esen a la o ien ación de un cue po ijo o el
ma co de coo denadas en un espacio idimensional. Una o ien ación a bi a ia del ma co B
ela i o al ma co A se puede consegui median e una o ación del ángulo θ sob e el eje A
de inido en el ma co A. Es o se ep esen a en la siguien e igu a, donde los ec o es
mu uamen e o ogonales xA, yA y zA y xB, yB y zB de inen los p incipales ejes de coo denadas de
los ma cos A y B.
Ilus ación 60: Rep esen ación g á ica de los ma cos A y B
El cua e nión que desc ibe la o ación 
, se de ine en la ecuación:

[ ] [ ]
Donde X, y, z de inen los componen es del ec o uni a io A en x, y, z espec i amen e
sob e el ma co A. El sis ema de supe índice y subíndice u ilizado se usa pa a indica el ma co
de e e encia como supe índice y el subíndice como el ma co que es á siendo desc i o.
Po ejemplo 
desc ibe la o ien ación del ma co B ela i o al ma co A y A es un ec o
desc i o en el ma co A. La a i mé ica de cua e niones gene almen e equie e que el cua e nión
que desc ibe una o ien ación sea an es no malizado. Po lo an o, es lo con encional que odos
los cua e niones engan longi ud unidad.
70
El conjugado de un cua e nión, indicado po “*”, se puede usa pa a in e cambia ma cos
ela i os desc i os po una o ien ación. Po ejemplo, 
es el conjugado de 
y desc ibe la
o ien ación de A ela i a al ma co B.


[ ]
El p oduc o de cua e niones, indicado po ⊗, se puede usa pa a de ini o ien aciones
compues as. Po ejemplo, pa a dos o ien aciones desc i as po 

, la o ien ación
compues a 
se de ine como: 
 ⊗

Pa a dos cua e niones a y b, el p oduc o de cua e nión se puede de e mina usando la egla
de Hamil on y se de ine como se puede e en la siguien e ecuación. Un p oduc o de cua e nión
no es conmu a i o, po lo que a ⊗ b no es igual que b ⊗ a.
⊗b [ ]⊗[ ]
[
]
Un ec o idimensional puede se o ado po un cua e nión u ilizando la elación desc i a
en la siguien e ecuación. A y B son el mismo ec o desc i o en el ma co A y en el ma co B
espec i amen e, donde cada ec o con iene un ce o como p ime elemen o pa a con e i lo
en un ec o de 4 ilas.

⊗
⊗ 
La o ien ación desc i a po 
se puede ep esen a como una ma iz de o ación
de inida po :
[
]
Los ángulos θ Φ en la llamada secuencia ae oespacial desc iben la o ien aci n del ma co
B po las o aciones secuenciales en alineaci n con el ma co A de al ededo de zB, θ sob e
yB y Φ sob e xB. La ep esen ación po ángulos de Eule de 
se de ine po :
a an
θ
Φ a an
71
8.2.2 O ien ación po a io angula
Un gi óscopo de 3 ejes mide el a io angula sob e los ejes x, y, z nomb ados ꙍx, ꙍy, ꙍz
espec i amen e. Si es os pa áme os (en adianes-1) se dis ibuyen en el ec o sꙍ, la de i ada
del cua e nión que desc ibe el a io de cambio de o ien ación del ma co e es e ela i o al
ma co del senso 󰇗
se puede calcula :
[ ]
󰇗

⊗
La o ien ación del ma co e es e ela i o al ma co del senso en el iempo ,
, puede
se compu ado in eg ando numé icamen e la de i ada de cua e nión
󰇗
siemp e que las
condiciones iniciales sean conocidas. En es as ecuaciones, Sꙍ es el a io angula medido en el
ins an e , ∆ es el pe iodo de mues eo y 
es la es imación de la o ien ación an e io . El
subíndice ꙍ indica que el cua e nión es calculado a pa i de a ios angula es.
󰇗

⊗

󰇗
8.2.3 O ien ación po obse ación ec o ial
Un acele óme o de 3 ejes mide la magni ud y di ección del campo g a i a o io en el ma co
del senso compues o po acele aciones lineales. De o ma pa ecida un magne óme o de 3
ejes mide la di ección y magni ud del campo magné ico de la ie a espec o al ma co del
senso compues o po el lujo magné ico local y dis o siones. En el con ex o de un il o de
o ien ación se asume que un acele óme o sólo medi á la g a edad y un magne óme o sólo
medi á el campo magné ico e es e.
Si la di ección de un campo e es e es conocida en el ma co e es e, una medida de la
di ección del campo en el ma co del senso pe mi i á el cálculo de una o ien ación del ma co
del senso ela i a al campo e es e. Sin emba go, pa a cualquie medida dada no hab á una
única solución de o ien ación, en su luga hab á in ini as soluciones ep esen adas po odas
las o ien aciones conseguidas debidas a la o ación de la au én ica o ien ación al ededo de un
eje pa alelo al campo.
72
Una ep esen ación ía cua e niones equie e encon a una solución comple a. Es o se
puede consegui a a és de la o mulación de un p oblema de op imización, donde la
o ien ación de un senso 
es alineada con una di ección p ede inida al campo del ma co
e es e Ed, con la di ección del ma co del senso Ss, u ilizando la ope ación de o ación
an e io men e desc i a. Po lo an o 
se puede encon a como la solución a:
Donde la ecuación siguien e de ine la unción obje o:
( 
󰆹
 )

⊗ 󰆹
⊗ 

Los componen es de cada ec o se desc iben como:

[ ]
󰆹
[ ]
 [ ]
Exis en a ios algo i mos de op imización pe o el algo i mo de g adien e descenden e es
uno de los más simples de implemen a y compu a . La siguien e ecuación desc ibe el
algo i mo pa a “n” i e aciones en una es imación de la o ien ación de 
y un amaño de
paso μ.

 (

󰆹
 )
‖ (

󰆹
 )‖
La siguien e ecuación compu a el g adien e de la solución de supe icie de inida po la
unción obje i o y su Jacobiano.
(

󰆹
 ) (

󰆹
) (

󰆹
 )
(

󰆹
 )
[
( )
( )
( )
]
(

󰆹
) [
]
Es as ecuaciones desc iben de o ma gene al el algo i mo aplicable a un campo p ede inido
en cualquie di ección. Sin emba go, si es posible asumi que la di ección del campo sólo iene
componen es en 1 ó 2 de los ejes p incipales del ma co de o ien ación global, las ecuaciones se
simpli ican.
79
El mensaje xbus educido queda al que (en o den de ecepción):
1. MID (1 by e) --> Iden i ica i o de mensaje.
2. LEN (1 by e) --> Longi ud en by es de los da os.
3. Da aID (2 by es) --> Iden i ica i o del da o en iado.
4. Da a_LEN (1 by e) --> Longi ud del da o en iado ( e e en e al Da aID)
5. Da os (Da a_LEN by es) --> Da os en iados
[...] --> (Puede en ia más da os y se epe i ía desde el pun o 3)
6. CheckSum (1 by e) --> CheckSum del mensaje suponiendo el BusID = 0xFF.
Ilus ación 63: Es uc u a de la ama SPI de MTi 1 - se ies
8.3.2.2 Opcode
El p ime by e que llega ía SPI es el "opcode". Según el opcode el o ma o del mensaje a ía.
Po lo an o, se ca aloga el mensaje p ime o según el opcode:
- OPCODE 0x01 --> Lec u a --> De inido po opcode
- OPCODE 0x02 --> Esc i u a --> De inido po opcode
- OPCODE 0x03 --> Esc i u a --> xbus educido
- OPCODE 0x04 --> Lec u a --> De inido po opcode
- OPCODE 0x05 --> Lec u a --> xbus educido
- OPCODE 0x06 --> Lec u a --> xbus educido
8.3.2.3 Con igu ación SPI
Modo 4 - hilos SPI:
- Chip Selec / Sla e Selec (SPI_nCS)
- Se ial Clock (SPI_SCK)
- Mas e da a In, Sla e da a Ou (SPI_MISO)
- Mas e da a Ou , Sla e da a In (SPI_MOSI)
El módulo u iliza el modo 3 de SPI:
Cap u a da os en el lanco ascenden e de eloj y p opaga los da os en el lanco descenden e.
(CPOL = 1 y CPHA=1). La in o mación se en ía MSB p ime o. El módulo u iliza un o ma o de 8
bi s.

80
Ilus ación 64: Funcionamien o SPI de MTi 1 - se ies
- T1: Tiempo en e lanco descenden e de Sla e Selec y p ime paque e en iado: Mín 4
us
- T2: Tiempo de en ío de un by e: Mín 4 us
- T3: Tiempo pa a en ío consecu i o: Mín 3 us
- Bi a e máximo del SPI: 2 Mbi s/s
8.3.2.4 Funciones de la lib e ía desa ollada
xsens_SPI_ini (s uc spi_de ice *m i_spi)
P ime o einicia la MTi 1-se ies accionando el pin ese du an e un iempo de e minado.
Realiza una comunicación con la IMU ía SPI en la que se en ía el OPCODE "PIPE_STATUS" de
modo que de uel a 2 uin 16 que se leen, pe o que po aho a quedan en el ol ido. Lo ideal se ía
espe a ecibi con i mación de que la IMU se ha einiciado (posible mejo a). Una ez se ha
e minado de ecibi la in o mación espe ada en el bus SPI la unción e mina. Necesi a como
a iables:
- Pun e o a la es uc u a del spi_de ice que aya a usa .
xsens_SPI_ac ualiza Medidas(s uc spi_de ice *m i_spi)
Llama a la unción "xbus_spi_ ead()" y le da in o mación del bus spi_de ice que se es á
u ilizando y el pin SS que necesi a á pa a ealiza su unción. Es posible amplia más adelan e
es a unción pa a que se enca gue de ac ualiza es uc u as de da os donde se almacene la
in o mación de los senso es. Necesi a como a iables:
- Pun e o a la es uc u a del spi_de ice que aya a usa .
xbus_spi_ ead(s uc spi_de ice *m i_spi)
Realiza llamadas a las unciones "xbus_spi_ eadPipeS a us()", "xbus_spi_ eadPipeNo i ()",
"xbus_spi_pa seDa a()" y les da la in o mación que necesi an pa a unciona .
xbus_spi_ eadPipeS a us(s uc spi_de ice *m i_spi)
Solici a ía SPI el amaño de la in omación p epa ada pa a en ia ía SPI po pa e de la
MTi 1-se ies, an o de mensajes de no i icación como de mensajes de mediciones. Gua da los
alo es en los a ays no i ica ionSize y measu emen Size. Necesi a como a iables:
- Pun e o a la es uc u a del spi_de ice que aya a usa .
81
xbus_spi_ eadPipeNo i (s uc spi_de ice *m i_spi)
Solici a y ecibe los mensajes de no i icación p epa ados de la MTi 1-se ies. Necesi a como
a iables:
- Pun e o a la es uc u a del spi_de ice que aya a usa .
xbus_spi_ eadPipeMeas(s uc spi_de ice *m i_spi)
Solici a y ecibe los mensajes de mediciones p epa ados de la MTi 1-se ies. Necesi a como
a iables:
- Pun e o a la es uc u a del spi_de ice que aya a usa .
xbus_spi_da aswapendian(uin 8_ * da a, uin 8_ leng h)
Cambia el o den de los by es (4 by es cada ez) del a ay que se le hayan en iado. Es
u ilizado po la unción "xbus_spi_pa seDa a". Necesi a como a iables:
- Pun e o al a ay de da os (En es e caso del da o que se quie e lee , no el a ay
comple o)
- Tamaño del da o a lee del a ay.
xbus_spi_pa seDa a(uin 8_ * da a, uin 8_ da aleng h)
Realiza el pa seo del mensaje de mediciones ecibido. La unción de momen o gua da los
da os ecibidos en sus a iables co espondien es, pe o se ía más con enien e que es as
a iables o masen pa e de una es uc u a de da os ya p ede inida (que odo es é más
o denado). Necesi a como a iables:
- Pun e o al a ay de da os (Mensaje leído del measu emen pipe)
- Tamaño del a ay de da os (Tamaño del mensaje ecibido po measu emen pipe)
82
Ilus ación 65: Diag ama de lujo del uncionamien o del Pa se de la ama SPI
83
8.4 Mic ocon olado ATMEGA2560
8.4.1 Diag ama de bloques del mic ocon olado
Ilus ación 66:Diag ama de bloques del Mic ocon olado
84
8.4.2 Pe i é ico SPI
Ilus ación 67: Diag ama de bloques del pe i é ico SPI del Mic ocon olado

85
Funcionalidad /SS y modos de uncionamien o:
Ilus ación 68: Funcionalidad del pin /SS y modos de uncionamien o
86
Regis os de con ol y es ado:
Ilus ación 69: Regis os de con ol de SPI
87
Ilus ación 70: Regis os de con ol y es ado
88
8.4.3 Pe i é ico USART
Fo ma o de las amas:
Ilus ación 71: Fo ma o de las amas
95
Regis os TWI:

96
97
98
99
8.5 Módulo ecep o GNSS
Desc ipción del o ma o de las amas NMEA:
100
Lis a de mensajes NMEA:

101
Desc ipción del mensaje NMEA que ac i a o desac i a el en ío de cada mensaje:
102
Se u ilizan dos amas GNSS pa a ob ene los da os necesa ios pa a el uncionamien o del
sis ema: GPGGA y GPMRC
103
104
Desc ipción del o ma o de los mensajes UBX:
111

112
8.6 MARG
Mapa de egis os del MPU 9250:
113
114
115
Regis os de con igu ación del acele óme o y gi óscopo:
116
Regis os de con igu ación del ByPass. Pe mi e accede a a és del bus I2C a los egis os del
magne óme o incluidos en el encapsulado:

117
Regis os de lec u a de acele óme o y gi óscopo:
118
119
120
127
8.7 APC220
8.7.1 Pinou del Módulo

128
8.7.2 Pa áme os de con igu ación
129
8.7.3 Especi icaciones écnicas del APC220
Como implemen a un PID con Mic ocon olado es
1

Abs ac o— Es e documen o a a el ema de los con olado es
PID en mic ocon olado es. Se empieza con una b e e esumen de
la eo ía de ás del con ol au omá ico y los con olado es PID
pa a e mina con la p og amación en mic ocon olado es de los
mismos.
En la p ime a sección se hace una b e e in oducción de lo
que es el con ol au omá ico. Después, en la segunda sección se
explican los undamen os de la eo ía de con ol mode na. En la
e ce a y cua a sección se e á una ap oximación in ui i a a lo
que es un con olado PID y su unning. En la quin a sección se
e á la o ma de implemen a un PID básico así como un mé odo
e icaz de co egi uno de sus de ec os más ecu en es, el wind-
up. En la sex a sección se en las conclusiones del documen o y
inalmen e, en la sép ima sección se en una se ie de e e encias
bibliog á icas del ema a ado en es e documen o.
I. INTRODUCCIÓN
l con ol au omá ico es una ama de la ingenie ía que se
ocupa del con ol de un p oceso en un es ado de e minado;
po ejemplo, man ene la empe a u a de una calde a, el umbo
de un cohe e o la elocidad de un mo o .
Los con olado es au omá icos compa an el alo de salida
de una plan a con el alo deseado (consigna) y p oduce una
señal de con ol que educe la des iación a ce o, o en su
de ec o a un alo pequeño. La o ma en que es e con ol
p oduce la señal de con ol ecibe el nomb e de acción de
con ol.
Es posible clasi ica en di e en es ipos de con olado es de
acue do a su acción de con ol:
a) Con olado es Todo o Nada
b) Con olado es P opo cionales (P)
c) Con olado es In eg ales (I)
d) Con olado es P opo cionales e In eg ales (PI)
e) Con olado es P opo cionales y de i a i os (PD)
) Con olado es P opo cionales, de i a i os e
in eg ales (PID)
En es e documen o se ha á especial én asis en los
con olado es PID.
II. INTRODUCCIÓN A TEORÍA DE CONTROL
Aquí se in en a á hace algo que no es an ecuen e de
encon a , da una isión in ui i a de la eo ía de con ol y
de la mo i ación que hay de ás, de o ma que sea ácilmen e
comp ensible, y sin ene que usa ans o madas de Laplace,
luga es de las aíces, diag amas de Bode, ni eo emas de
Nyquis .
A. El sis ema
Se denomina sis ema a una ep esen ación simpli icada de la
ealidad que es abs aída pa a pode abaja con él.
No malmen e se ep esen a una pa e de la ealidad que
aislamos.
Se puede conside a sis ema a casi cualquie cosa. La
p esión de una calde a, el sis ema de clima ización de un
coche, el equipo de bombeo de un depósi o, los con oles de
un a ión, la p opulsión de una na e espacial, un obo ... has a
el sis ema diges i o de un pa o.
Un sis ema, al que no malmen e se le ha á e e encia como
plan a, iene una o a ias en adas que ep esen an acciones
sob e las que se puede ac ua . También iene una o a ias
salidas que son los pa áme os que se pueden obse a y
medi . En gene al un sis ema iene muchas en adas y salidas,
pe o no malmen e se an a simpli ica a las más impo an es.
La plan a, además, iene un cie o compo amien o. Es
deci , ealiza una acción end á una de e minada
consecuencia sob e sus salidas. Po supues o, el
compo amien o puede se odo lo complicado que se pueda
imagina . Puede se no lineal, ene ine cias, incluso e asos
pu os (un iempo en e que se aplica la acción y el sis ema
ac úa), e ec os en e a ias en adas.
El compo amien o de la plan a ambién puede se abs aído
y modelado de o ma simpli icada, en lo que se conoce como
unción de as e encia. Y se puede ep esen a , po ejemplo,
median e una ecuación, un diag ama de bloques, una
ep esen ación g á ica o ecuencial.
Cómo implemen a un PID con
Mic ocon olado es
A. Elgezabal Núñez, BiSKY Team, Bilbao
E
Como implemen a un PID con Mic ocon olado es
2
Además la plan a puede ene pe u baciones, es deci ,
modi icaciones en el en o no que modi ican su
compo amien o. Po ejemplo, en el edi icio la empe a u a
ex e io a a cambia , en el coche se puede subi o baja una
cues a, y en el embalse se puede pone a llo e .
En esumen, un sis ema es un modelo de una po ción del
uni e so, que iene en adas y salidas, cuya elación en e
és as sigue un de e minado compo amien o y que es á
some ido a posibles pe u baciones.
.
Y es o se suele ep esen a así:
Figu e 1: Rep esen ación de un sis ema
B. El con olado
El con olado es un elemen o que se pone an es de las
en adas de nues a plan a pa a ac ua sob e ellas con
obje o de consegui que la salida enga unas de e minadas
ca ac e ís icas.
Po ejemplo, si se quie e con ola la empe a u a de un
edi icio ac uando sob e la calde a, la elocidad del coche
ac uando sob e el mo o , o el ni el del depósi o ac uando
sob e el bombeo.
Lógicamen e, la en ada sob e la que se a a ac ua debe
ene una in luencia en la salida que se quie e con ola . Si
se es á con olando una a iable que es o almen e
independien e de lo que se es á midiendo, no se consegui á
nada.
El con olado puede se desde una pe sona, has a un
sis ema analógico, o un sis ema digi al. En sis emas
au omá icos lógicamen e se hace e e encia a algún ipo de
máquina con olada po el sis ema de o ma au ónoma sin
in e ención manual.
El con olado , ep esen ado jun o al sis ema, queda ía así.
Figu e 2: Rep esen ación de Con olado y sis ema
C. Ca ac e ís icas a la salida
Lo más e iden e es que la salida enga un alo
de e minado que se denomina consigna. Po ejemplo, se
quie e que la empe a u a sea de 24ºC, que el coche aya a
80km/h, o que el ni el del agua de un embalse sea 175m.
Po supues o, la consigna no iene que se la misma
siemp e. Se puede que e que la empe a u a sea 17ºC du an e
la noche y 24ºC du an e el día, que el coche pase a 50km/h al
pasa po una a esía, o que e acia a 160m embalse po que
engo que suminis a agua po able a la ed.
Pe o, además de la consigna, hay o as ca ac e ís icas
deseadas pa a la salida. En gene al, se a a que e que la
salida sea es able a la go plazo, que con e ja al alo de la
consigna, que el iempo de espues a sea ápido, que no oscile,
que no sob epase el alo de consigna en ningún momen o.
No se a a pode ene las odas a la ez, así que se iene
que llega a un comp omiso en e odas. Aunque, llegado el
caso puede, que se es é dispues o a sac i ica unas espec o a
o as.
Po ejemplo, quizás no impo a que la empe a u a de un
edi icio alcance b e emen e 24.5ºC, si así el iempo de
espues a es meno . Pe o si a 180m de agua el embalse se
ompe, inundando un pueblo y ma ando a cien os de pe sonas,
quizás no sea ole able el desbo damien o y se p e ie a que
suba el ni el len o pe o segu o.
Po an o, las ca ac e ís icas deseadas pa a la salida
dependen o almen e del sis ema y de lo que se quie a
hace , y po an o el con olado que se iene que emplea .
D. Lazo Ce ado
Pa a que un con olado uncione iene que ene
ealimen ación. Básicamen e el con olado iene que pode
moni o iza la salida que in en a con ola di ec a o
indi ec amen e.
Sin ealimen ación, un con olado no es un con olado .
Es á cla o que pa a con ola una salida hay que pode
medi la. Así que se coge la salida y se compa a con la
consigna pa a e el e o que hay. Se emplea es e e o
como en ada del con olado . Es o se exp esa así.
Como implemen a un PID con Mic ocon olado es
3
Figu e 3 Rep esen ación de un Lazo Ce ado (L.C.)
El e o en e la medición y la consigna puede se bien
po que aún no se ha conseguido que la salida alcance a la
consigna, o po que se ha cambiado la consigna.
Con ealimen ación el sis ema global (con olado +
plan a) se compo a de o ma o almen e dis in a. La salida
puede desde ende obedien emen e a la consigna, a pone se a
oscila como una loca has a que ompa algo.
Y la di e encia en e una salida (éxi o absolu o en el sis ema
de con ol) y la o a ( o u a, despido, y posible des ucción del
uni e so) depende o almen e del con ol que se diseñe.
III. APROXIMACIÓN INTUITIVA AL CONTROLADOR PID
En es e capí ulo se e á el con olado PID, uno de los
con olado es más ex endidos po su sencillez y po se capaz
de consegui un buen compo amien o en una g an a iedad de
si uaciones.
En el capí ulo an e io se ie on algunas gene alidades y
de iniciones sob e eo ía de sis emas.
Po supues o, no se a a en a en p o undidad en los
de alles del PID, dado que es ema muy ex enso (e
in e esan e). Si exis e in e és se puede consul a la abundan e
documen ación disponible.
En es e capí ulo se in en a da una isión in ui i a del
con olado y de la mo i ación que explica su
compo amien o. Sin usa ecuaciones, ni ma emá icas. De
hecho, es una de las cosas geniales del PID, que no es
necesa io sabe en de alle cómo unciona, pa a hace que
uncione.
A. ¿Qué es el PID?
El con olado PID es uno de los más empleados en la
indus ia pa a el con ol de sis emas ealimen ados. Algunas
de sus o alezas son su sencillez y que es capaz de da
un buen compo amien o en una g an a iedad de
si uaciones sin necesidad de conoce con de alle la plan a a
con ola .
El con olado PID es conocido desde hace iempo. Sus
p ime os usos da an de 1911, y su p ime análisis eó ico de
1922 de la mano de Nicolas Mino sky. En esos iempos, el
con ol PID e a exclusi amen e analógico. Sin emba go,
esul a ácil implemen a en p og amación un PID digi al y los
cálculos que equie e son sencillos y e icien es. Además, es
" ela i amen e sencillo" ex apola la eo ía de los PID
analógicos a su equi alen e digi al.
Pese a su popula idad, hay que deci que ac ualmen e el PID
no es el mejo con olado disponible. Pe o en la mayo ía de
los casos es más que su icien e. Po o o lado, muchos de los
con olado es más "mode nos" no dejan de se e siones
mejo adas de un PID, como po ejemplo las dis in as amilias
basadas en PID con pa áme os adap a i os.
B. Funcionamien o del PID
El algo i mo PID (p opo cional, in eg al, de i a i o) es á
o mado po la suma de es componen es, P opo cional,
In eg al, y De i a i o. Ma emá icamen e, un con olado PID
iene la siguien e o mulación.
Cada componen e del PID es "independien e" de los
demás, en el sen ido de que cada uno calcula una salida de lo
que "pa a él" debe ía hace pa a ob ene la espues a
adecuada.
Los es componen es se suman pa a da la salida del
con olado . Cada uno cumple una cie a unción y mejo an
cie a pa e de la espues a. Y cuando los es componen es
abajan jun os, en la p opo ción adecuada, consiguen un g an
compo amien o.
Figu e 4: PID en un Lazo Ce ado
Cada componen e iene un pa áme o Kp, Ki y Kd,
espec i amen e. Es os pa áme os indican la ponde ación (o
"la ue za") que iene en el esul ado inal. Que la espues a
del PID sea buena, cosas o as, mue e y des ucción, eside
en el ajus e co ec o de es os es pa áme os.

Como implemen a un PID con Mic ocon olado es
4
Y aquí iene la pa e "g aciosa" del PID. En los es
pa áme os, si se pone un alo muy bajo no se a a no a el
e ec o del componen e en la salida. Y si se pone demasiado
al o… pues eso, mue e, des ucción, cosas o as, e c.
Es más, en la espues a global del con olado los es
componen es abajan jun os e in luyen uno sob e o os, po lo
que no ale con ajus a cada uno de los pa áme os de
o ma independien e. Exis e una cie a "zona" den o de los
es pa áme os, donde el compo amien o es más o menos
bueno.
Lógicamen e, queda cla o que la di icul ad (que ampoco
es pa a an o) de un PID es ajus a los pa áme os K, Ki y Kd,
pa a que el compo amien o sea bueno.
Como se ha explicado, el con olado PID se basa en es
componen es, PID. Su o aleza eside en el papel que cada
uno de es os componen es iene en la espues a. A modo
esumen:
 El componen e p opo cional eacciona al p esen e.
 El componen e in eg al eacciona al pasado, y
apo a "memo ia" al con olado .
 El componen e de i ado eacciona al u u o, y
apo a "p edicción" al con olado
C. Componen e P opo cional – P
1) Mo i ación
La mo i ación del componen e p opo cional segu amen e
sea el más in ui i o de explica . Si se en a en la habi ación y
en 12ºC, pa ece lógico que hay que da le más a la cale acción
que si pone 23ºC.
Pues esa es la mo i ación del componen e p opo cional,
que iene sen ido que se aplique más acción cuan o más lejos
es á del alo deseado, y ice e sa.
2) Fo mulación
El componen e p opo cional se calcula simplemen e como
un ac o K mul iplicado po el e o (di e encia en e consigna
y alo eal).
3) Compo amien o
El ac o p opo cional iene una g an in luencia en
la elocidad de espues a del sis ema. Si se iene una K
pequeña, el sis ema a a a da mucho en alcanza la consigna,
po que le es á aplicando poco al ac uado . Si se aumen a, se
consigue disminui el iempo de espues a.
Pe o si aumen a la K demasiado se puede sob epasa la
consigna u oscila .
Si se iene una K demasiado g ande, se le a a da
demasiado a la cale acción. Así al e 18ºC, se le da a la
cale acción y el siguien e alo es 25. se ha pasado. Baja la
cale acción, 21, aho a se ha pasado hacia abajo. Sube la
cale acción, 23. Bien, ya lo iene.
Pe o si lle a una K aún más g ande, pod ía pasa que al e
18º le dé MUCHO a la cale acción, y lo siguien e que se ea
pod ía se 32ºC. ¡Os as, se he pasado una ba ba idad, BAJA
LA CALEFACCIÓN! 14º ¡OSTRAS SUBE SUBE! 45º... 8º...
56º... Acaba de hace que el sis ema oscile y sea ines able.
O a ca ac e ís ica es que, en gene al, el componen e
p opo cional no elimina po comple o el e o a la go
plazo. En es e ejemplo, supongamos que a 22ºC y una
“posición” de la cale acción de e minada, la ene gía que
apo amos al edi icio es exac amen e la que necesi a el edi icio
pa a man ene la empe a u a. Se hab ía conseguido que la
empe a u a sea es able, pe o como la “posición” de la
cale acción iene dada po el e o (que se ha es abilizado)
nunca se pod á subi los 2ºC que al an.
D. Componen e In eg al – I
1) Mo i ación
La unción del componen e in eg al quizás sea el más
complicado de explica den o del PID. Imagina que se es á
en la sala, y hay 22ºC. El p opo cional dice que se ponga la
cale acción en una “posición” y el con olado lo pone ahí.
Pasa un minu o, pasa o o, pasa o o... y eso sigue en 22ºC,
2ºC po debajo de los que se quie e.
El componen e in eg al es el que dice... ¿oye chicos, igual
hay que subi un poco la cale acción no? Po que lle amos un
buen a o en 22ºC sin mo e la cale acción, y eso no iene
pin a de que aya a i se. Pues esa oz con an o sen ido, que
eacciona a la memo ia del e o pasado, es el componen e
in eg al.
2) Fo mulación
El componen e in eg al aplica una acción que es
p opo cional a la in eg al del e o a lo la go del iempo.
Equi alen emen e, esponde p opo cionalmen e a la suma de
odos los e o es an e io es.
G á icamen e co esponde con el á ea ence ado bajo la
cu a del e o , que ambién es lo mismo que el á ea en e la
salida y la consigna. En el campo disc e o, la in eg al se
sus i uye po un mé odo disc e o pa a su cálculo, como el
cálculo median e ec ángulos o apecios.
Como implemen a un PID con Mic ocon olado es
5
3) Compo amien o
El componen e in eg al pe mi e al con olado elimina
po comple o el e o a la go plazo. Sin emba go, si Ki es
muy pequeño, el sis ema a da á mucho en elimina el e o .
Po o o lado, hay que ene en cuen a que el é mino
in eg al únicamen e esponde al á ea en e la salida y la
consigna. Una consecuencia de ello es que si hemos
acumulado (po ejemplo) un e o po es a po debajo de la
consigna, la única o ma que iene el é mino in eg al de
compensa lo es es a un iempo po encima.
Y e ec i amen e, el componen e in eg al iene endencia
a sob epasa y oscila , más incluso que el é mino
p opo cional.
E. Componen e De i a i o – D
1) Mo i ación
El componen e de i ado ambién es sencillo de explica .
Suponiendo que se es á anquilamen e con la empe a u a a la
consigna a 24ºC. ¡Todo pe ec o! La empe a u a baja a 23ºC,
luego a 22ºC, y se sube un poqui o la palanca. Todo a al y
como se espe a.
Aho a imaginando que, cuando se es aba a 24ºC la
empe a u a baja a 18ºC de golpe. Os as, eso es una buena
bajada. Se subes la cale acción. Ma ca 12ºC... Han abie o
una en ana, pe o ¡La empe a u a sigue bajando muy ápido!
Siguien e alo 2ºC.
Ésa es la unción del componen e de i ado, eacciona an e
a iaciones del e o (po cambio de consigna o de la
a iable). Po que no es lo mismo es a a 12ºC y subiendo
len amen e, que si la empe a u a se es á desplomando a oda
elocidad. Es deci , el é mino de i a i o esponde a la
elocidad de cambio del e o .
2) Fo mulación
El componen e de i ado se calcula de o ma p opo cional
a la de i ada del e o espec o del iempo en el momen o
p esen e. Equi alen emen e, esponde p opo cionalmen e a la
di e encia en e el e o ac ual y el e o en el ins an e an e io .
En el campo disc e o, la de i ada se sus i uye po la
di e encia en e el e o ac ual y el e o an e io , di idido po
el iempo de mues eo (o se ob ia es a di isión po comple o y
se incluye en el coe icien e Kd, si el iempo de mues eo es
cons an e).
3) Compo amien o
El componen e de i ado mejo a la espues a gene al de
muchos sis emas pa a alo es de Kd mode ados. Sin
emba go, si se excede de Kd se e á que apa ece una al a de
"sua idad" en la espues a, y o os compo amien os " a os".
Además, el componen e de i ado esponde muy mal al
uido de la medición. El uido, una a iación de al a
ecuencia, supone a iaciones muy ápidas, aunque sean de
pequeña ampli ud. Es as a iaciones son ampli icadas po el
componen e de i ado.
Y, po úl imo, el componen e de i ado a eces es un poco
"bes ia" po que pide acciones muy g andes. Imagina , po
ejemplo, un cambio ins an áneo en la consigna. El componen e
de i ado demanda ía una acción in ini a que el accionado no
pod ía sa is ace . Como el accionado no pod ía da la acción
que solici a el con olado , se end ían des iaciones espec o a
lo calculado, o incluso se pod ía daña el accionado .
IV. TUNNING DEL PID
En unción del alo que se ponga a los pa áme os, se
ob end á una buena espues a, o una espues a len a, u
oscilan e, o incluso ... mue e, des ucción.
Pa a hace lo más complicado, el e ec o de los ac o es en
la espues a no es independien e. Su apo ación se mezcla y
en emezcla. Po lo que no se pueden ajus a los pa áme os
de "uno a uno".
El éxi o del con olado depende o almen e del ajus e.
Pa a ello, es impo an e que se conozcan las ca ac e ís icas y
e ec os de cada ac o en la espues a del sis ema.
A. Resumen de los e ec os del PID
1) Componen e P opo cional
 Poco K, espues a len a.
 Mucho K, desbo damien o, oscilación, e incluso
ines abilidad.
 No consigue elimina el e o en es aciona io.
2) Componen e In eg al
 Elimina el e o es aciona io
 Demasiado Ki, oscilación e ines abilidad
3) Componen e De i a i a
 Mejo a el compo amien o gene al
 Demasiado Kd, compo amien o " a o" en la salida
 Muy sensible al uido
 Muy sensible a cambios b uscos en el e o
(pe u baciones o cambio de consigna)
Como implemen a un PID con Mic ocon olado es
6
B. Ajus ando el PID
Exis en muchas o mas de ajus a un PID, algunas más o
menos eó icas.
1) Ca ac e ización de la plan a
Consis e en de e mina la unción de ans e encia de la
plan a. P ác icas habi uales son aplica una en ada escalón o
ecuencial, y analiza las ca ac e ís icas de la salida.
Después se puede ope a ma emá icamen e pa a ealiza un
cálculo exac o del con olado pa a la espues a eque ida.
2) Aplicación de eglas de sin onización
De o ma simila , consis e en aplica una de e minada
en ada al sis ema, y medi la salida. A con inuación, se
aplican una de las muchas eglas de sin onización (ejemplo
amoso, eglas Ziegle -Nichols) pa a ob ene los pa áme os
del PID.
3) Au o Tunning
En simila al an e io , pe o ealizado de o ma au omá ica
po el con olado . Los egulado es PID han a anzado mucho
y, en la ac ualidad, muchos ienen es upendos algo i mos de
au o ajus e.
P ecisamen e, es a capacidad de au o ajus e es uno de los
mo i os del éxi o come cial de los con olado es PID.
4) Ajus e manual (Heu ís ico)
Finalmen e, es á el ajus e "a mano". Que, aunque suene
a al, es una opción muy álida y muy popula . En muchas
ocasiones, un ope ado humano puede consegui un ajus e an
bueno (o incluso supe io ) a un Au o uning gené ico.
C. Ajus e manual del PID
Pa a ajus a el PID manualmen e se debe pode ac ua
sob e la en ada del sis ema, sob e los pa áme os del PID, y
isualiza la espues a. Y aquí, " isualiza " no signi ica
necesa iamen e pone un osciloscopio. En el caso de un mo o ,
po ejemplo, se puede " e " la espues a del sis ema
simplemen e mi ando cómo gi a (o cómo suena). Hay dos
mé odos p incipalmen e:
1) Ajus e P opo cional/In eg al/De i a i o
 Ajus a K has a que el sis ema sob e pase o empiece a
sob e oscila .
 Ajus a Ki has a elimina e o es aciona io.
 Aumen a Kd has a que empiezan a pasa cosas a as.
2) Ajus e P opo cional/De i a i o/In eg al
 Ajus a K has a que el sis ema sob e pase o empiece a
sob e oscila
 Aumen a Kd has a que empiezan a pasa cosas a as
 Ajus a Ki has a elimina e o es aciona io
Con los dos se an a consegui esul ados simila es, en
úl imo é mino. El au o aconseja el p ime o.
El mo i o es que el é mino de i a i o es muy sensible al
uido de la medición y, po an o, es di ícil de ajus a en un
caso eal.
V. IMPLEMENTAR UN CONTROLADOR CON
MICROCONTROLADORES
Aho a oca deja la eo ía, desempol a el eclado y e
cómo se implemen a un con olado PID en un
mic ocon olado . A o unadamen e, es bas an e sencillo hace
un PID básico.
A. PID a Mano
P ime o se a a e cómo implemen a un con olado
sencillo. En ealidad, el código no es excesi amen e complejo.
Supongamos que el con olado oma la en ada de la a iable
con olada en A0, y e ec úa la salida median e una señal PWM
en el pin 3.
El código de un PID básico pod ía se el siguien e:
// Asignaciones pins
cons in PIN_INPUT = A0;
cons in PIN_OUTPUT = 3;
// Cons an es del con olado
double Kp=2, Ki=5, Kd=1;
// a iables ex e nas del con olado :
double Inpu , Ou pu , Se poin ;
// a iables in e nas del con olado :
unsigned long cu en Time, p e iousTime;
double elapsedTime;
double e o , las E o , cumE o , a eE o ;
oid se up()
{
Inpu = analogRead(PIN_INPUT);
Se poin = 100;
}
oid loop()
{
pidCon olle .Compu e();
// lee una en ada del con olado
Inpu = analogRead(PIN_INPUT);
Como implemen a un PID con Mic ocon olado es
7
// calcula el con olado
Ou pu = compu ePID(Inpu );
delay(100);
// esc ibi la salida del con olado
analogW i e(PIN_OUTPUT, Ou pu );
}
double compu ePID(double inp)
{
// ob ene el iempo ac ual
cu en Time = millis();
// calcula el iempo anscu ido
elapsedTime = (double)(cu en Time - p e iousTime);
// de e mina el e o en e la consigna y la medición
e o = Se poin - Inpu ;
// calcula la in eg al del e o
cumE o += e o * elapsedTime;
// calcula la de i ada del e o
a eE o = (e o - las E o ) / elapsedTime;
// calcula la salida del PID
double ou pu = kp*e o + ki*cumE o + kd* a eE o ;
// almacena e o an e io
las E o = e o ;
// almacena el iempo an e io
p e iousTime = cu en Time;
e u n ou pu ;
}
Como se puede obse a , no es demasiado di ícil. Hay una
unción compu ePID() que ealiza odo el abajo. En es a
unción se calcula el iempo anscu ido en e llamadas, que
es necesa io pa a calcula an o la de i ada como la in eg al
del e o .
A con inuación, se compa a la en ada del con olado
con la consigna pa a de e mina el e o y se ealizan las
'pseduo' in eg ales y de i adas (su equi alen e en disc e o).
Finalmen e, se calcula la espues a del sis ema median e la
ó mula del PID, y se gua dan los alo es pa a el siguien e
ciclo.
Sin emba go, aunque es comple amen e uncional, es e
con olado PID es bas an e simple. Po an o, iene cie as
ca encias an e si uaciones que ocu en ecuen emen e en la
ealidad.
B. Rese wind-up
Rese windup es una ampa en la que caen muchos
p incipian es. Ocu e cuando el PID c ee que es á haciendo
algo que no puede. Po ejemplo, un PWM que acep a alo es
en e 0 y 255. El PID po de ec o no sabe es o. Si c ee que los
alo es 300, 400, 500 unciona án, p oba á esos alo es
espe ando consegui lo que necesi a. Dado que en ealidad el
alo es á sa u ado a 255 segui á in en ando alo es cada ez
más al o sin llega a ninguna pa e.
El p oblema se p esen a en o ma de e asos a os.
C. La Solución
1) P ime paso
Exis en di e sas mane as pa a mi iga el Wind-up, pe o la
o ma más in ui i a es indica le al PID cuáles son los lími es
de salida. En el código que se e más adelan e hay una
unc ion Se Ou pu Limi s. Cuando uno de los limi es es
alcanzado, el PID deja de suma (In eg a ).
2) Segundo paso
Hay aún di e encia en e lo que el PID c ee que es á
en iando y lo que en ealidad en ía. Es o es debido al
componen e P opo cional y (en meno medida) al e mino
de i a i o.
Aunque el componen e in eg al ha sido sa u ado de o ma
segu a, P y D es án aún con ibuyendo al wind-up. Es o puede
se inacep able en según que aplicación. Así que como
segundo paso se sa u a el alo de salida ambién.
/*wo king a iables*/
unsigned long las Time;
double Inpu , Ou pu , Se poin ;
double ITe m, las Inpu ;
double kp, ki, kd;
in SampleTime = 1000; //1 sec
double ou Min, ou Max;
oid Compu e()
{
unsigned long now = millis();
in imeChange = (now - las Time);
i ( imeChange>=SampleTime)
{
/*Compu e all he wo king e o a iables*/
double e o = Se poin - Inpu ;
ITe m+= (ki * e o );
i (ITe m> ou Max) ITe m= ou Max;
else i (ITe m< ou Min) ITe m= ou Min;
double dInpu = (Inpu - las Inpu );
/*Compu e PID Ou pu */
Ou pu = kp * e o + ITe m- kd * dInpu ;
i (Ou pu > ou Max) Ou pu = ou Max;
else i (Ou pu < ou Min) Ou pu = ou Min;
/*Remembe some a iables o nex ime*/
las Inpu = Inpu ;
las Time = now;
}