Procesador aeronáutico de trayectoria y orientación
Abstract
El equipo BiSky Team de la Escuela de Ingeniería de Bilbao se plantea el uso de aeromodelos autogobernados para mantener una supervisión activa durante los lanzamientos de cohetes, así como para el reconocimiento de áreas poco accesibles con intención de localizar los cohetes cuando ya hayan aterrizado. La idea del proyecto es la siguiente: la aviónica dentro del aeromodelo, mediante una unidad de instrumentación y un sistema G.N.S.S., es capaz de conocer y corregir tanto su posición como orientación y velocidad respecto al plano terrestre, actuando sobre el motor y las superficies de control. Antes del despegue se carga una lista de puntos G.N.S.S. por los que se encargará de navegar en orden de forma automática. En caso necesario, el supervisor puede cambiar el modo de vuelo y manejar remotamente la aeronave. Esta aeronave servirá de plataforma para diferentes cargas (radio repetidores, cámaras, experimentos… etc.) durante su vuelo.
Full text
Curso: 2020 - 2021 Fecha: Junio, 2021> Alumno/Alumna: Elgezabal Núñez, Aritz Director/Directora: Ortiz Álvarez-Cienfuegos, Javier 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. Introducción.................................................................................................................................................................. 3 2. Contexto .......................................................................................................................................................................... 3 3. Objetivos y planificación temporal ..................................................................................................................... 4 4. Desarrollo....................................................................................................................................................................... 5 4.1 Aeromodelo ........................................................................................................................................................... 5 4.1.1 Funcionamiento de un Aeromodelo de ala fija ............................................................................. 5 4.2 Perfil aerodinámico ....................................................................................................................................... 7 4.3 Aviónica ................................................................................................................................................................ 13 4.3.1 Hardware .................................................................................................................................................... 13 4.3.2 Software ....................................................................................................................................................... 20 5 Resultados.................................................................................................................................................................... 42 5.1 Validación del algoritmo ............................................................................................................................... 42 5.1.1 XSens MTi-7 ............................................................................................................................................... 42 5.1.2 MPU-9250 ................................................................................................................................................... 42 5.1.3 Prueba en banco de pruebas ............................................................................................................... 43 5.1.4 Pruebas dinámicas .................................................................................................................................. 45 5.2 Tiempos de los subprocesos........................................................................................................................ 46 5.3 Prueba con aeromodelo ................................................................................................................................ 48 6 Conclusiones ............................................................................................................................................................... 52 6.1 Autonomía ........................................................................................................................................................... 52 6.1.1 Turbofan o Turbojet ............................................................................................................................... 52 6.1.2 Paneles solares ......................................................................................................................................... 52 6.2 Aeromodelo ........................................................................................................................................................ 53 6.2.1 Estabilidad .................................................................................................................................................. 53 6.2.2 Sustentación, entrada en pérdida e hipersustentadores ........................................................ 53 6.2.3 Velocidad ..................................................................................................................................................... 53 6.3 Posicionamiento global ................................................................................................................................. 54 6.3.1 Precisión ...................................................................................................................................................... 54 6.3.2 Pérdida de la señal .................................................................................................................................. 54 6.4 Algoritmos de control..................................................................................................................................... 55 6.4.1 Control Guiado .......................................................................................................................................... 55
2 6.4.2 Control de trayectoria............................................................................................................................ 55 6.4.3 Modo Repetidor ........................................................................................................................................ 55 6.5 Lazo de control de velocidad aerodinámica ......................................................................................... 56 7 Bibliografía .................................................................................................................................................................. 57 7.1 Aerodinámica y Aeronáutica ....................................................................................................................... 57 7.2 Software y Control ........................................................................................................................................... 57 7.3 Hardware ............................................................................................................................................................. 57 8 Anexos ........................................................................................................................................................................... 58 8.1 Micromod Estándar M.2 ................................................................................................................................ 58 8.2 Filtro de orientación ....................................................................................................................................... 69 8.2.1 Representación por “Cuaterniones” ................................................................................................ 69 8.2.2 Orientación por ratio angular ............................................................................................................ 71 8.2.3 Orientación por observación vectorial ........................................................................................... 71 8.2.4 Algoritmo de fusión de filtros ............................................................................................................. 74 8.2.5 Compensación de la distorsión magnética ................................................................................... 76 8.2.6 Compensación de la deriva giroscópica ......................................................................................... 76 8.3 XSens MTi-7 ........................................................................................................................................................ 78 8.3.1 Procedimiento de envío SPI (MTi 1 – series) ............................................................................... 78 8.3.2 MTSSP Protocolo serial síncrono ...................................................................................................... 78 8.4 Microcontrolador ATMEGA2560 ............................................................................................................... 83 8.4.1 Diagrama de bloques del microcontrolador ................................................................................ 83 8.4.2 Periférico SPI ............................................................................................................................................. 84 8.4.3 Periférico USART ..................................................................................................................................... 88 8.4.4 Especificaciones del TWI compatible con I2C ............................................................................. 94 8.5 Módulo receptor GNSS ................................................................................................................................... 99 8.6 MARG .................................................................................................................................................................. 112
3 1. Introducción El equipo BiSky Team de la Escuela de Ingeniería de Bilbao se plantea el uso de aeromodelos autogobernados para mantener una supervisión activa durante los lanzamientos de cohetes, así como para el reconocimiento de áreas poco accesibles con intención de localizar los cohetes cuando ya hayan aterrizado. La idea del proyecto es la siguiente: la aviónica dentro del aeromodelo, mediante una unidad de instrumentación y un sistema G.N.S.S., es capaz de conocer y corregir tanto su posición como orientación y velocidad respecto al plano terrestre, actuando sobre el motor y las superficies de control. Antes del despegue se carga una lista de puntos G.N.S.S. por los que se encargará de navegar en orden de forma automática. En caso necesario, el supervisor puede cambiar el modo de vuelo y manejar remotamente la aeronave. Esta aeronave servirá de plataforma para diferentes cargas (radio repetidores, cámaras, experimentos… etc.) durante su vuelo. 2. Contexto Durante los lanzamientos de cohetes de BiSky Team, el equipo se ha encontrado con retos a la hora de recuperar los lanzadores. El reto principal a superar es la pérdida de cobertura del radioenlace de telemetría. Cuando el cohete cae es posible que caiga detrás de un obstáculo y que éste produzca sombra (electromagnética). El cohete transmite su posición GNSS vía enlace de telemetría, es por eso que se presenta esta solución. El aeromodelo con control automático puede realizar la búsqueda del aparato una vez haya caído, para así servir de plataforma a un repetidor entre el cohete y la estación de tierra. Una vez encontrado el aparato, el aeromodelo puede permanecer en el aire sin necesidad de intervención, simplificando y facilitando así la labor de búsqueda y recuperación. Ilustración 1: Propuesta al problema del radioenlace
4 3. Objetivos y planificación temporal En este proyecto, centrado en la electrónica industrial y automática, los objetivos generales son: 1. Diseñar la aviónica de un aeromodelo. 1. Escoger el tipo de aeromodelo. 2. Escoger la instrumentación. 3. Escoger y programar el filtro digital de orientación. 4. Desarrollar un programa funcional y universal. Se comienza recopilando información teórica acerca de aerodinámica general, de la instrumentación utilizada en la electrónica de aviación y métodos de orientación electrónicos. Se busca una relación de compromiso entre los métodos más económicos y los más computacionalmente eficientes. Una vez escogidos los métodos a utilizar, se accede a información sobre aeronavegación y se simplifica el problema. Esta simplificación trae consigo una dificultad añadida, puesto que el sistema de referencia del sistema de orientación y el de trayectoria no tienen por qué ser el mismo, por lo tanto, será necesario buscar una forma de relacionar, directa o indirectamente, ambos sistemas de referencia. En la siguiente etapa se diseñan las maniobras de vuelo y el/los lazo(s) de control que gobernarán la aeronave. El modelo del avión se hará lo más simple y universal posible, con el fin de que pueda ser reutilizado en futuras simulaciones externas a este proyecto. Después, se diseña un aeromodelo sencillo, cuyas principales características serán su estabilidad y lenta reacción. Esto facilitará las primeras pruebas y la afinación de los PIDs que se programarán. Finalmente, se termina el software y se realizan tanto pruebas estáticas como dinámicas del prototipo. Ilustración 2: Diagrama Gantt, en semanas
5 4. Desarrollo El desarrollo del proyecto se divide en 2 partes principalmente: primero el diseño y desarrollo de un aeromodelo de ala fija, segundo el diseño y programación de la aviónica que gobierna el aparato. 4.1 Aeromodelo Con la intención de partir de un aeromodelo cuya estabilidad es conocida, se plantea diseñar desde cero un aeromodelo de ala fija para baja velocidad de vuelo. Para realizar el diseño se realiza una iteración de diseño y en función de los resultados de las simulaciones, se modifican las diferentes dimensiones y posiciones de las superficies aerodinámicas. Esto inicia un nuevo proceso iterativo hasta el momento en que el aeromodelo cumpla los requisitos exigibles para posibilitar el desempeño de sus funciones. Para simular el diseño del aeromodelo se utiliza el programa XFLR5. Primero se introducen diferentes perfiles aerodinámicos para analizarlos mediante el programa. Después, los datos recabados por el programa se utilizan para realizar las simulaciones pertinentes a la hora de diseñar el avión. Antes de empezar con el diseño del aeromodelo es importante entender el funcionamiento de los aviones. 4.1.1 Funcionamiento de un Aeromodelo de ala fija También conocidos como aeroplanos o aviones, son vehículos capaces de volar siendo más pesados que el aire, cuya característica principal para poder hacerlo es el uso de alas fijas. La razón de que las alas permitan al avión volar es el concepto aerodinámico de “Downwash”. En aerodinámica en general se define downwash como el cambio en la dirección del aire desviado por la acción aerodinámica de una superficie (como la pala del rotor de un helicóptero o el ala de un avión en movimiento), como parte del proceso de producción de sustentación. Es por eso que un ala debe verse como una herramienta diseñada para lanzar el aire a su paso hacia abajo. Esto es algo bastante intuitivo si miramos la forma del ala y vemos lo que ocurre con el efecto Coanda en una cucharilla. Ilustración 3: Efecto Coanda demostrado con una cuchara y agua.
6 “El efecto Coanda es el fenómeno físico por el cual una corriente de fluido, gaseosa o líquida, tiende a ser atraída por una superficie vecina a su trayectoria. El término fue acuñado por Albert Metral en honor al ingeniero aeronáutico rumano Henri Coandă, quien descubrió el efecto en su prototipo de un avión a reacción.” – Wikipedia 06/04/2021 Que un avión se sostenga en el aire o que un perfil aerodinámico genere sustentación es un ejemplo perfecto de la aplicación de la tercera ley de movimiento de Newton: Al crear el flujo descendente, como reacción aparece una fuerza de igual magnitud y sentido opuesto, que se aplica sobre el avión: es la fuerza de sustentación. La sustentación es una fuerza y, como tal, se puede medir aplicando la segunda ley de Newton. La sustentación producida por un ala es proporcional a la cantidad de aire que ésta desvía por unidad de tiempo, multiplicada por la velocidad vertical del aire que ha sido desviado. De forma similar al principio de operación de un helicóptero, uno puede aumentar la sustentación de un ala aumentando la velocidad vertical del aire, la cantidad de aire que se desvía, o una combinación de las dos. Ilustración 4: Principio del Downwash ilustrado
7 4.2 Perfil aerodinámico Previo al diseño del aeromodelo es necesario escoger los perfiles aerodinámicos que se utilizarán en sus alas, timón de dirección y timón de profundidad. 4.2.1 Nomenclatura de los perfiles NACA Existen diversas publicaciones donde se catalogan las propiedades de familias de perfiles y se facilita información sobre sus características aerodinámicas. Posiblemente los perfiles más utilizados en aeronáutica sean los perfiles NACA, cuya nomenclatura de 4 cifras se explica en la tabla siguiente, recogida de [1]. Familia Perfiles NACA de cuatro cifras Espesor Es la misma ley de espesores que la de perfiles Clark Y y Göttingen 398, con un factor de afinidad que proporciona el valor del espesor relativo. Curvatura Dos parábolas de segundo grado tangentes en el punto de tangente horizontal Nomenclatura NACA XYZZ (ejemplo NACA 2415) X 100f/c donde f es la flecha máxima de la curvatura y c la cuerda. Y 10x f/c donde xf es la posición de la flecha máxima de la curvatura. ZZ 100 e/c, donde e es el espesor máximo y c la cuerda. Ilustración 5: NACA de 4 dígitos
8 4.2.2 Simulación Para los timones de profundidad y dirección se utiliza el perfil aerodinámico NACA 0012. Un perfil aerodinámico simétrico que hace de control en este aeromodelo. Ilustración 6: Perfil NACA 0012 introducido en XFLR5 Para las alas se ha escogido el perfil aerodinámico Eppler E214. Es un perfil aerodinámico para bajos valores de Reynolds. Para poder simular este perfil aerodinámico mediante el programa XFLR5, se ha utilizado la información disponible en: https://m-selig.ae.illinois.edu/ Ilustración 7: Perfil Eppler E214 introducido en XFLR5 Para continuar, se simulan ambos perfiles aerodinámicos ante diferentes números de Reynolds y ángulos de ataque. Para ello se configura un conjunto de análisis para Reynolds entre 100.000 y 150.000 a incrementos de 10.000, y para ángulos de ataque entre -2 y 16 grados con incrementos de un grado. Ilustración 8: Configuración del análisis
15 4.3.1.2 Módulo Microcontrolador El módulo Microcontrolador incorpora un microcontrolador, los componentes pasivos necesarios para su funcionamiento y las conexiones al conector M.2 Estándar correspondientes. Cualquier microcontrolador con la memoria suficiente, velocidad de reloj que permita la ejecución del programa y sus interrupciones, y buses de comunicación I2C, SPI y UART puede utilizarse en este módulo. ATMEGA2560 Para el prototipo se ha escogido un microcontrolador ATMEGA2560 alimentado a 3.3 Voltios. Éste es un microcontrolador de 8 bits que tiene como trabajo el cálculo de orientación y trayectoria de la aeronave a partir de los sensores disponibles, así como el mando sobre las superficies de control. En la siguiente imagen se puede observar un esquemático reducido de la alimentación para este microcontrolador. Ilustración 19: Conexiones de alimentación del ATMEGA2560
16 4.3.1.3 Módulo de instrumentación El módulo de instrumentación es el módulo donde se encuentran el receptor GNSS y el MARG (IMU + Magnetómetro). Este módulo necesita recoger del conector M.2 Estándar: alimentación, bus I2C (para la comunicación entre Microcontrolador y MARG) y bus UART (para la comunicación entre Microcontrolador y receptor GNSS). MPU-9250 El IC MPU-9250 es un conjunto IMU + magnetómetro que se vende como módulo para plataformas de desarrollo de hardware y es aproximadamente 200 veces más barato que un XSens MTi-7 (AHRS que se ha utilizado como referencia comercial durante los ensayos). Debido a su bajo coste y su fácil implementación en Arduino (ya existen librerías que lo manejan), este módulo ha sido escogido como unidad MARG de P.A.T.O. Ilustración 20: Esquema del módulo MARG MPU9250 Receptor GNSS El receptor GNSS de la familia NEO-6 ofrece los datos a través de UART, a una velocidad de 9.600 bps. Los datos transmitidos son sentencias definidas en el protocolo NMEA (National Marine Electronics Association). La intensidad de corriente necesaria es de unos 37 mA en modo de medición continuo. Se utiliza un modelo NEO-6M, puesto que se alimentará a 3.3 V (También sería posible utilizar los modelos NEO-6Q que funcionan con la misma tensión). 4.3.1.4 Módulo de Actuadores Los servomotores encargados de gobernar las superficies de control del aeromodelo necesitan tanto alimentación como control, procedente de la aviónica. Se conectan a este módulo.
17 4.3.1.5 Módulo de telemetría El Módulo de telemetría se encarga de mantener una comunicación bidireccional con la estación de tierra. Frecuencia A la hora de escoger la frecuencia de operación es importante tener en cuenta los efectos de difracción, absorción y refracción. Se pueden separar en 4 grupos de propagación: ondas terrestres, directas, de satélite e ionosféricas. Las ondas terrestres (Surface Waves): Al enviar más allá del horizonte, en teoría debería haber sombra (electromagnética), las ondas inducen otras corrientes en la tierra haciendo que éstas aprovechen el efecto de refracción sobre esta nueva zona y que lleguen muy lejos. La distancia depende del tipo de suelo, sobre el que se inducen las corrientes. Las frecuencias de las ondas terrestres se encuentran entre 30 Hz y 3 MHz. Ilustración 21: Surface Waves Las ondas directas tienen como característica que se propagan hasta que se encuentran un obstáculo o el horizonte. Se utilizan en todo el espectro electromagnético. La distancia máxima de propagación dependerá de la altura a la que se encuentren las antenas. √ √ Ondas ionosféricas (Sky Waves) y de satélite (Space Waves): La atmósfera tiene varias capas. Durante los 20 primeros kilómetros se encuentra la troposfera, después está la estratosfera durante otros 30 kilometros y después está la ionosfera; esta parte de la atmósfera está situada entre los 50 y 400 km y es menos densa. La ionosfera hace de capa eléctrica, al reaccionar a la radiación solar, y por lo tanto afecta a las transmisiones electromagnéticas. Desde los 50 km están la capa D de la ionosfera, la capa E, la capa F1 y la capa F2. De noche, al no reaccionar al sol sólo está la capa F (F1 + F2. La más externa).
18 Para las ondas de UHF (mayores de 300 MHz) la ionosfera es transparente. Éstas son las Ondas de satélite o Space Waves. Las frecuencias muy bajas son absorbidas por la ionosfera. De 3 KHz a 3 MHz las ondas rebotan en la ionosfera (reflexión). Es importante tener en cuenta que la variación de las capas entre noche y día hace que la distancia de rebote cambie, siendo mayor de noche. Cualquier onda reflejada cambiará su polarización y por lo tanto se verá atenuada. Las llamadas Ondas ionosféricas, de frecuencia entre 30 KHz y 30 MHz, refractan en la ionosfera. Ilustración 22: Esquema con las diferentes ondas Dado que la aeronave debe estar siempre en Línea de Visión, según las normativas actuales, se escoge una frecuencia entre 3 MHz y 30 GHz para el funcionamiento de la telemetría. En caso de que aparezca un obstáculo puntual (un pájaro, un globo…) entre ambas antenas el radioenlace podría verse afectado y por tanto, se decide que la frecuencia será preferiblemente baja. Esto se debe a que a frecuencias más bajas, los obstáculos en Línea de Visión pueden ser mitigados gracias al efecto de la difracción.
19 Modulación GFSK Se escoge una modulación GFSK para hacer el trabajo. Teniendo todo lo anterior en cuenta se escoge una frecuencia de trabajo 431 MHz. Para este trabajo se utiliza un APC220 cuyo datasheet se encuentra disponible en los anexos. Se utiliza otro APC220 con un adaptador de UART a USB para recibir la telemetría mediante el monitor serie de Arduino IDE. Ilustración 23: Módulos APC220 + adaptador USB/UART para funcionar de telemetría
20 4.3.2 Software El software a desarrollar se separa principalmente en 2 partes diferentes. Por un lado está el Control Guiado de la aeronave, que permite la aeronavegación mediante la orientación respecto a tierra del aeromodelo, y por otro lado está el Control de Trayectoria, que mediante un sistema GNSS es capaz de conocer su posición y trayectoria para seguir una serie de puntos GNSS previamente programados. Estos dos lazos se superponen, siendo el Control de Trayectoria el que da las consignas al Control Guiado. Ilustración 24: Diagrama simplificado de los lazos de control de P.A.T.O. 4.3.2.1 Control Guiado El Control Guiado, es el lazo de control encargado de orientar la aeronave y por lo tanto de gobernar las superficies de control para, por medio de la instrumentación a bordo, corregir las reacciones del avión hasta obtener la orientación solicitada. Navegación aérea para aeromodelos de 2 G.D.L. Un aeromodelo de 2 grados de libertad utiliza el timón de dirección y el timón de profundidad para controlar su vuelo. Más detalles acerca de la aeronavegación en [2]. Para controlar el ángulo de inclinación (pitch en inglés, normalmente θ) utiliza el timón de profundidad. La acción de esta superficie de control disminuye o aumenta la fuerza descendente creada por la parte trasera del avión. Una mayor fuerza descendente en la cola, producida por un timón de profundidad hacia arriba, fuerza a la cola del avión a ir hacia abajo y a la nariz a ir hacia arriba y la velocidad se reduce. Una disminución de fuerza descendente en la cola, producida por un timón hacia abajo, permite que la cola se eleve y la nariz baje.
21 Para controlar la dirección del avión se utiliza el timón de dirección. Es importante que para que este control funcione el ángulo superior de las alas debe ser inferior a 180º. Esto es llamado ángulo de diedro y necesita ser positivo o lo que es lo mismo, un ángulo inferior a 180º por la parte de arriba. Un efecto adicional del ángulo de diedro es la contribución a la estabilidad del avión. La acción del timón de dirección hace girar el aeromodelo en su eje de deriva (yaw en inglés, normalmente Ψ), lo que hace que exista un deslizamiento entre la orientación del avión y la dirección del mismo (y por lo tanto de la dirección de la que reciben el viento las alas). Este deslizamiento provoca (entre otras cosas) que el ángulo de ataque del ala hacia donde se gira disminuya mientras que el ángulo de ataque del ala contraria aumente. Esta modificación de ángulos de ataque hace que la sustentación del ala con más ángulo de ataque aumente provocando que el avión gire en su eje de alabeo (roll en inglés, normalmente Φ). Cuando el avión se encuentra girado en su eje de alabeo y la dirección del viento coincide con la orientación del avión, el ángulo de ataque en ambas alas es el mismo, por lo que la fuerza de sustentación es la misma. Dado que el avión está girado y una de las alas no realiza su fuerza en vertical (porque está girado) el avión vuelve de forma natural a su posición de reposo (posición estable para un vuelo recto y nivelado). Ilustración 26: Demostración del autonivelado mediante ángulo de diedro Ilustración 25: Ángulo de diedro
22 Lazo de control para dos grados de libertad El lazo de control para dos grados de libertad se divide en 3 bloques principales: Control Guiado 2 G.D.L., Planta y Realimentación AHRS. A la hora de realizar la planta del aeromodelo, se plantea un modelo matemático simplificado, donde el aeromodelo es un sólido rígido. Éste reacciona a los pares generados por una fuerza constante, sobre la superficie frontal aparente de las superficies de control. Adicionalmente estos pares son sometidos a perturbaciones que representan, no sólo perturbaciones durante el vuelo, sino las auténticas fuerzas aerodinámicas a las que el avión estará sometido también en un vuelo normal. Es posible hacer esto porque se conoce la gran estabilidad de este aeromodelo, lo que permite que cualquier irregularidad (como es el modelo excesivamente simplificado que se propone aquí) se corrija automáticamente. Ilustración 27: Planta simplificada de aeromodelo de 2 G.D.L. Las superficies de control se mueven gracias a servomotores; éstos se representan como lazos cerrados (junto con una saturación como limitación mecánica de posición) tal y como se ve en la siguiente ilustración: Ilustración 28: Servomotores con limitación mecánica de posición
23 Los servomotores se encuentran dentro del bloque “Control guiado 2 G.D.L.”. Este bloque recibe consignas de giro e inclinación que posteriormente se adaptan. El error entre estas consignas y el ángulo Euler correspondiente llega a un PID que gobierna los servomotores. Cabe destacar que el valor de la consigna una vez adaptado no puede superar valores de seguridad asignados heurísticamente con el fin de que el aeromodelo trabaje con patrones de vuelo ya conocidos y que, por lo tanto, no entre en pérdida. Ilustración 29: Bloque "Control Guiado 2 G.D.L." El control sigue los principios de navegación aérea para aeronaves de 2 G.D.L. anteriormente comentados. Para ello, el ascenso será establecido por la inclinación del avión y por lo tanto el error entre consigna de elevación adaptada y la inclinación será el que gobierne el timón de profundidad. Por otro lado el giro será establecido por el Roll del avión y será el error entre éste y la consigna quien determine la posición del timón de dirección. Ilustración 30: Lazo de control 2 G.D.L. simplificado en Simulink
24 Navegación aérea para aeromodelos de 3 G.D.L. Un aeromodelo de 3 grados de libertad utiliza timón de profundidad, timón de dirección y alerones para controlar su vuelo. Más detalles acerca de la aeronavegación en [2]. Para controlar el ángulo de inclinación utiliza el timón de profundidad. La acción de esta superficie de control disminuye o aumenta la fuerza descendente creada por la parte trasera del avión, igual que en el aeromodelo de 2 G.D.L. En aerodinámica, el giro es una maniobra básica muy compleja e implica el uso de timón de dirección, timón de profundidad y alerones. Durante el vuelo recto y nivelado, la sustentación total actúa en vertical y directamente opuesta a la gravedad. Al alabear el avión, la sustentación (que sigue siendo perpendicular al eje transversal del aeromodelo) actúa ahora en un plano inclinado. Si se descompone esta sustentación en dos vectores (uno vertical y otro horizontal), el componente vertical de la sustentación se opone a la gravedad. El componente horizontal actúa como fuerza centrípeta, tirando del avión hacia el centro de un eje imaginario. Esta fuerza centrípeta lo impulsa a girar alrededor de dicho eje, contribuyendo la sección de cola a mantener el aeromodelo alineado con el viento relativo en una trayectoria curvada. Ilustración 31: Fuerzas que actúan en un giro Por lo tanto, se alabea el avión para inclinar la sustentación. Esta inclinación de la sustentación hace que además de soportar el peso del avión, se provea de fuerza centrípeta, que mantiene al avión alrededor del eje vertical de giro, contrarrestando la fuerza centrífuga que tiende a expulsar al avión de la trayectoria curvada.
31 4.3.5 Programación del microcontrolador A la hora de programar el Microcontrolador es importante tener en cuenta que hay parte del programa que se ejecuta normalmente y otra (gran) parte del programa se ejecuta de forma cíclica y temporizada. Por lo tanto se empieza por el programa principal. Antes de eso se añaden una serie de librerías: #include <Wire.h> //Librería para trabajar con el bus I2C #include <math.h> //Librería necesaria para el filtro de orientación #include <SPI.h> //Librería para trabajar con el bus SPI #include <TimerOne.h> //Librería para trabajar con interrupciones cíclicas Configuración Los datos obtenidos del receptor GNSS NEO-6 están en formato $GPGSV, $GPVGT, $GPGLL, $GPGSA, $GPRMC y $GPGGA, que son algunas de las secuencias disponibles en el protocolo NMEA. Estas secuencias tienen esta forma: $GPGSV,NoMsg,MsgNo,NoSv,{,sv,elv,az,cno}*cs $GPVTG,cogt,T,cogm,M,sog,N,kph,K,mode*cs $GPGLL,Latitude,N,Longitude,E,hhmmss.ss,Valid,Mode*cs $GPGSA,Smode,FS{,sv},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,Latitude,N,Longitude,E,FS,NoSV,HDOP,msl,m,Altref,m,DiffAge,DiffStation*cs
32 Antes de comenzar a obtener los datos, conviene configurar el módulo GNSS para que transmita las tramas que convienen al proyecto. En este caso se escogen dos: $GPRMC y $GPGGA. //Configuración del GNSS GPS.begin(9600) while (GPS.available()) GPS.read(); // Se inhabilita la trama GPGLL strcpy(linea,”$PUBX,40,GLL,0,0,0,0,0,0*”); CheckSum(); GPS.println(linea); // Se inhabilita la trama GPGSA linea[10] = ‘S’; linea[11] = ‘A’; linea[25] = 0; CheckSum(); GPS.println(linea); // Se inhabilita la trama GPGSV linea[11] = ‘V’; CheckSum(); GPS.println(linea); // Se inhabilita la trama GPVGT linea[9] = ‘V’; linea[10] = ‘G’; linea[11] = ‘T’; linea[25] = 0; CheckSum(); GPS.println(linea); // Sólo quedan habilitadas las secuencias $GPGGA y $GPRMC. // Selección del puerto UART y su configuración linea[0] = 0xB5; linea[1] = 0x62; // Cabecera UBX linea[2] = 0x06; linea[3] = 0x00; // Configuración de Puerto linea[4] = 0x14; linea[5] = 0x00; // Longitud (Set/Get) linea[6] = 0x01; // Puerto UART 1 linea[7] = 0x00; // Reservado 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; // Baudrate 38.400 bps (1) linea[16] = 0x00; linea[17] = 0x00; // Baudrate 38.400 bps (2) linea[18] = 0x03; linea[19] = 0x00; // inProtoMask linea[20] = 0x03; linea[21] = 0x00; // outProtoMask linea[22] = 0x00; linea[23] = 0x00; // Reservado linea[24] = 0x00; linea[25] = 0x00; // Reservado linea[26] = 0x8F; linea[27] = 0x70; // CheckSum for (i = 0; i < 28; i++) // Transmite GPS.write(linea[i]); // Los 28 bytes GPS.end(); // Cierra el Puerto GPS GPS.begin(38400); // Abre el puerto GPS a 38.400 bps
33 Para configurar el MPU9250 se puede utilizar la librería o mediante registros. Para ahorrar memoria del Microcontrolador se utilizan los registros directamente: //Se reinicia el módulo para asegurar un estado conocido: Wire.begin(); // Inicio del I2C Wire.beginTransmission(MPU); // Conexión al MPU Wire.write(0x6B); // Escribe registro Power Management 1 Wire.write(0); // RESET Wire.endTransmission(TRUE); // Fin de transmisión Se empieza por la configuración del giróscopo. El fondo de escala se puede configurar a 250 dps, 500 dps, 1000 dps o 2000 dps, escribiendo en su registro el valor 0x00, 0x08, 0x10 o 0x18 respectivamente. // Configuración del Giróscopo: Wire.beginTransmission(MPU); // Conexión al MPU Wire.write(0x1B); // Registro Config. Giroscopo Wire.write(0x08); // Escribe fondo de escala 500 dps para giros. Wire.endTransmission(TRUE); // Fin de la transmisión Después se continúa con la configuración del acelerómetro. El fondo de escala del acelerómetro se puede ajustar a 2 G, 4 G, 8 G o 16 G, escribiendo en su registro 0x00, 0x08, 0x10 ó 0x18 respectivamente. //Configuración del acelerómetro: Wire.beginTransmission(MPU); // Conexión al MPU Wire.write(0x1C); // Registro Config. Acelerometro Wire.write(0x08); // Escribe fondo de escala a 4 Gs Wire.endTransmission(TRUE); // Fin de la transmisión Por último, el magnetómetro no es necesario configurarlo, pero es necesario acceder a él. Es necesario que se escriba el bit “BYPASS_EN” para poder tener acceso a él. El magnetómetro es un segundo chip dentro del encapsulado del módulo IMU y por lo tanto hay que avisar a éste, que es el dueño del I2C, de que se quiere usar ese dispositivo: //Acceso al chip interno magnetómetro: Wire.beginTransmission(MPU); // Conexión al MPU Wire.write(0x37); // Registro INT Pin/Bypass Wire.write(2); // Activar el bypass (Acceso a AK8963) Wire.endTransmission(TRUE); // Fin de la transmisión Se configura la interrupción cíclica al periodo de muestreo escogido en las simulaciones con Simulink utilizando la librería TimerOne.h: Timer1.attachinterrupt(ejecucion_ciclica); Timer1.initialize(PERIODO_MUESTREO_US); //Valor en us
34 Con esto se termina el setup y se comienza con el loop del programa: Para interpretar la secuencia se puede programar un parser que separe los valores que sean de interés. Para ello se presenta un código que aprovecha las particularidades de ASCIIZ para hacer más fácil el trabajo. Para esto se buscan los separadores (comas) y se sustituyen por el carácter de control null (0x00), a la vez que se escribe en cada posición del array de punteros “dato” la dirección de inicio de cada uno de ellos, hasta encontrar el asterisco, que indica la llegada del checksum. Con esto cada puntero apunta a un dato de la sentencia leída. i = 5; j = 0; dato[j ++] = &(linea[3])]; while (linea[i]) != ‘*’) { if (linea[i] == ‘,’) { dato[j++] = &(linea[i + 1]); linea[i] = 0; } i++; if (i > 89) break; } Ahora se comprueba si la trama es $GPRMC o &GPGGA. En caso de ser &GPRMC se obtiene el valor de la dirección del vector de trayectoria. Si la trama es &GPGGA, se recoge la posición actual (latitud, longitud y altura). Todo se guarda en un array de nombre “registro”. if (j > 8) { if (!strcmp(dato[0], ‘GGA’)) { // Sentencia GGA ********************************************************** strcpy(registro[HORA], dato[1]); if (dato[6][0] != ‘0’) { strcpy (registro[LATITUD_N], dato[2]); strcpy (registro[LATITUD_A], dato[3]); strcpy (registro[LONGITUD_N], dato[4]); strcpy (registro[LONGITUD_A], dato[5]); strcpy (registro[ALTITUD], dato[9]); } } else if (!strcmp(dato[0], ‘RMC’)) { //Sentencia RMC ********************************************************** strcpy(registro[HORA], dato[1]); if (dato[6][0] != ‘0’) { strcpy (registro[CURSO], dato[7]); } } }
35 Ahora, para evitar que la interrupción coja los datos a medias se utiliza un flag y un array llamado registro_copia en el que volcar los datos. De este modo se evita que cuando se esté escribiendo en uno de ellos, los datos recogidos por el algoritmo cíclico puedan ser erróneos. flag_volcado = 0; strcpy (registro_copia[LATITUD_N], registro[LATITUD_N]); strcpy (registro_copia[LATITUD_A], registro[LATITUD_A]); strcpy (registro_copia[LONGITUD_N], registro[LONGITUD_N]); strcpy (registro_copia[LONGITUD_N], registro[LONGITUD_N]); strcpy (registro_copia[ALTITUD], registro[ALTITUD]); flag_volcado = 1; Con esto se acaba la función loop. Sólo quedan funciones de usuario. La primera de ellas es la función CheckSum la cual se encarga de calcular el CheckSum de los mensajes de configuración del receptor GNSS. void CheckSum() { cs = 0; for (posicion = 1; linea[posicion] != ‘*’; posicion++) // XOR con cs para cada byte cs = cs ^ línea[posición]; l = cs % 16; // Hexadecimal bajo h = (cs – l) / 16; // Hexadecimal alto l += 48; // Conversión a char if (l > 57) // Si es mayor a 9 l += 7; // Escribe letra (A-F) h += 48; // Conversión a char if (h > 57) // Si es mayor a 9 h += 7; // Escribe letra (A-F) linea[++ posición] = h; // Se escribe el CheckSum linea[++ posición] = l; linea[++ posición] = 0; } Después va la función del filtro de orientación: A la hora de implementar este algoritmo con Arduino IDE, hace falta incluir la librería math.h. Se empieza por crear una función que utilice los valores recogidos por el MARG para su funcionamiento: void madgwick_actualizarMARG(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { Dentro de la función se empieza por declarar todas las variables que se utilizarán. Todas las variables que se declaran son tipo float. Se obtiene el valor de la derivada del cuaternión (el cuaternión está formado por variables globales). qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); qDot2 = 0.5f * ( q0 * gx + q2 * gz - q3 * gy); qDot3 = 0.5f * ( q0 * gy - q1 * gz + q3 * gx); qDot4 = 0.5f * ( q0 * gz + q1 * gy - q2 * gx);
36 Lo siguiente se calcula sólo en caso de que el valor recibido del acelerómetro sea correcto, por lo tanto se utiliza una condición: if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) Primero se normalizan las medidas de acelerómetro y magnetómetro: //Normalización del acelerómetro recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Normalización del magnetómetro recipNorm = invSqrt(mx * mx + my * my + mz * mz); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; Ahora, para evitar calcular repetidamente, se calculan una serie de variables auxiliares. _2q0mx = 2.0f * q0 * mx; _2q0my = 2.0f * q0 * my; _2q0mz = 2.0f * q0 * mz; _2q1mx = 2.0f * q1 * mx; _2q0 = 2.0f * q0; _2q1 = 2.0f * q1; _2q2 = 2.0f * q2; _2q3 = 2.0f * q3; _2q0q2 = 2.0f * q0 * q2; _2q2q3 = 2.0f * 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 variables auxiliares ya calculadas, se empieza por calcular la referencia terrestre mediante el campo magnético: 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 = sqrtf(hx * hx + hy * hy); _2bz = -_2q0mx*q2 + _2q0my*q1 + mz*q0q0 + _2q1mx*q3 - mz*q1q1 + _2q2*my*q3 - mz*q2q2 + mz*q3q3; _4bx = 2.0f * _2bx; _4bz = 2.0f * _2bz;
37 Se continúa con el algoritmo gradiente descendente y su normalización: // Algoritmo correctivo de gradiente descendente s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - 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.5f - q1q1 - q2q2) - mz); s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - 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.5f - q1q1 - q2q2) - mz); s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // Normalizar magnitud s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm; Finalmente se aplica la realimentación: // Aplicar realimentación qDot1 -= beta * s0; qDot2 -= beta * s1; qDot3 -= beta * s2; qDot4 -= beta * s3; Ahora se sale de la condición de los acelerómetros (por lo tanto, lo que se calcule a partir de ahora se ejecutará sean correctos los valores o no). Se incluye la derivada del cuaternión al cuaternión: q0 += qDot1 * invSampleFreq; q1 += qDot2 * invSampleFreq; q2 += qDot3 * invSampleFreq; q3 += qDot4 * invSampleFreq; Y finalmente se normaliza el cuaternión: recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; anglesComputed = 0;
38 Con esto las variables q0, q1, q2 y q3 tienen el valor del cuaternión de orientación calculado. Para que esta función pueda ser calculada se utiliza la función invSqrt. En las siguientes líneas de código puede verse la función invSqrt: float invSqrt(float x) { float halfx = 0.5f * x; float y = x; long i = *(long *)&y; i = 0x5f3759df - (i>>1); y = *(float *)&i; y = y * (1.5f - (halfx * y * y)); y = y * (1.5f - (halfx * y * y)); return y; } Adicionalmente, se utilizarán los ángulos de Euler durante los experimentos, ya que resultan más sencillos de entender, por lo tanto la siguiente función será la encargada de realizar el cambio de cuaterniones a ángulos de Euler: void computeAngles() { roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2); pitch = asinf(-2.0f * (q1*q3 - q0*q2)); yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); anglesComputed = 1; } La siguiente es la función que se ejecutará cíclicamente, primero se recogen los datos del MPU 9250, empezando por los acelerómetros. Habrá que realizar un ajuste en función de la sensibilidad escogida en el acelerómetro. void ejecucion_ciclica() { //Acelerómetros: Wire.beginTransmission(MPU); Wire.write(0x3B); //Solicita acelerómtetros Wire.endTransmission(FALSE); Wire.requestFrom(MPU, 6, true); // Acelerómetro OX: lectura = (Wire.read() <<8 | Wire.read()) / ajuste[accel]; frac2alfa(); strcpy(registro[ACCEL_X], inter); // Acelerómetro OY: lectura = (Wire.read() <<8 | Wire.read()) / ajuste[accel]; frac2alfa(); strcpy(registro[ACCEL_Y], inter); // Acelerómetro OZ: lectura = (Wire.read() <<8 | Wire.read()) / ajuste[accel]; frac2alfa(); strcpy(registro[ACCEL_Z], inter);
39 Se repite el mismo procedimiento para los giróscopos: // Giróscopos: Wire.beginTransmission(MPU); Wire.write(0x43); //Solicita giróscopos Wire.endTransmission(FALSE); Wire.requestFrom(MPU, 6, true); // Giróscopo OX: lectura = (Wire.read() <<8 | Wire.read()) / ajuste[accel]; frac2alfa(); strcpy(registro[GYRO_X], inter); // Giróscopo OY: lectura = (Wire.read() <<8 | Wire.read()) / ajuste[accel]; frac2alfa(); strcpy(registro[GYRO_Y], inter); // Giróscopo OZ: lectura = (Wire.read() <<8 | Wire.read()) / ajuste[accel]; frac2alfa(); strcpy(registro[GYRO_Z], inter); Para los magnetómetros, tras pedir acceso al chip del magnetómetro se lee el registro 0x02 hasta que contenga el bit 1 activo, en este momento se puede acceder a los registros que contienen los valores de los magnetómetros. Wire.beginTransmission(MAG); Wire.write(0x0A); Wire.write(0x11); Wire.endTransmission(true); data_ready=0; while (! (data_ready & 1)) { Wire.beginTransmission(MAG); Wire.write(0x02); Wire.endTransmisission(false); Wire.requestFrom(MAG, 1, TRUE); data_ready = (Wire.read()) } Wire.beginTransmission(MAG); Wire.write(0x03); Wire.endTransmission(false); Wire.requestFrom(MAG, 6, true); itoa((Wire.read() | Wire.read() <<8), sbuffer, 10); strcpy(registro[MAG_X], sbuffer); itoa((Wire.read() | Wire.read() <<8), sbuffer, 10); strcpy(registro[MAG_Y], sbuffer); itoa((Wire.read() | Wire.read() <<8), sbuffer, 10); strcpy(registro[MAG_Z], sbuffer); Una vez obtenidos los datos se llama a la función del filtro de orientación, a la se darán los datos para calcular la orientación. Se le solicita también que haga la conversión a ángulos de Euler al final: //Calcular orientación: madgwick_actualizarMARG(registro[GYRO_X], registro[GYRO_Y], registro[GYRO_Z], registro[ACCEL_X],registro[ACCEL_Y],registro[ACCEL_Z], registro[MAG_X],registro[MAG_Y],registro[MAG_Z]); Conversión a ángulos de Euler: madgwick_computeAngles();
40 Las variables con los ángulos de Euler son variables globales, por lo que el resto de funciones pueden acceder a ellas. Después se procede a programar los PIDs necesarios para el control. Para programar PIDs en microcontroladores se sigue el artículo “Cómo implementar un PID con Microcontroladores” incluido en los anexos. El PID se ejecuta en una interrupción periódica, que asegura que la frecuencia de muestreo es siempre la misma. Por lo tanto, para el PID, en el bucle principal no hay nada. Todo se encuentra dentro de la propia interrupción (a excepción de la declaración de variables). En caso de saturar la salida, este PID puede dar problemas, por lo que se le incluye un anti wind-up para evitar este problema. Se empieza por el PID que gobierna el timón de profundidad: // Cálculo de controlador: unsigned long now_pitch = millis(); int timeChange_pitch = (now_pitch – lastTime_pitch); double error_pitch = altura_consigna_pitch - pitch; ITerm_pitch += (ki_pitch * error_pitch); If (ITerm_pitch > outMax) ITerm_pitch = outMax; else if (ITerm_pitch < outMin) ITerm_pitch = outMin; double dInput_pitch = (pitch – lastInput_pitch); // Calcular salida del PID: Output_pitch = kp_pitch * error_pitch + ITerm_pitch – kd_pitch * dInput_pitch; //Se satura la salida para que el número no siga creciendo If (Output_pitch > outMax) Output_pitch = outMax; else if (Output_pitch < outMin) Output_pitch = outMin; /*Memoria para el próximo ciclo*/ lastInput_pitch = pitch; lastTime_pitch = now_pitch; // escribir la salida del controlador analogWrite(TIMON_PROFUNDIDAD, output);
47 Por un lado, si da tiempo a entregar todo al buffer, la transmisión seguirá sin intervención del microcontrolador, tal y como se ve en la ilustración anterior. En caso contrario, habrá un retardo, entre dos de los bytes transmitidos por radio, de 15,7 ms. En cualquier caso la comunicación no se ve mermada y los tiempos se cumplen. Ilustración 47: Representación del tiempo de los subprocesos con interrupción intermedia 020 40 60 80 100 Radio GNSS Procesado MARG
48 5.3 Prueba con aeromodelo Debido a problemas con la impresora 3D, las primeras pruebas de vuelo se realizan con un aeromodelo comercial (Una réplica del Piper J-3 Cub concretamente). Ilustración 48: Piper J-3 Cub Tanto las pruebas de vuelo recto y nivelado como de giro son satisfactorias y los algoritmos de control hacen correctamente su trabajo. Pese a esto, se observa un movimiento extraño a la hora de volar con vientos fuertes. Cuando no hay mucho viento, el aeromodelo se dirige directamente hacia el punto de destino, tal y como cabe esperar. Ilustración 49: Vuelo sin viento
49 Cuando un viento constante y fuerte golpea al aeromodelo, éste empieza a actuar raro: comienza a girar en dirección al viento para después empezar a hacer zig-zag. Las conclusiones de los experimentos son las siguientes. Primero hay que tener en cuenta el escenario, el avión va directamente al punto marcado y recibe viento fuerte desde un costado. Ilustración 50: Avión en vuelo recibe viento fuerte Para compensar el cambio de trayectoria, éste gira contra el viento. Ilustración 51: Compensación de la trayectoria
50 Si el viento es lo suficientemente fuerte, el avión girará más aún. Este giro excesivo desemboca en que el algoritmo de control de trayectoria solicite un giro en sentido contrario. Cuando este segundo giro se excede nuevamente el sentido de giro vuelve a cambiar. Ilustración 52: Reacción del avión volando contra fuertes vientos La razón de este comportamiento se debe a que la velocidad del viento es mayor a la velocidad de movimiento (con respecto al aire) del aeromodelo. Este hecho hace que la suposición de que la diferencia entre el vector de trayectoria y el de orientación sea despreciable no se cumpla, debido a que el vector de trayectoria bien podría tener una diferencia de 180º con la orientación de la aeronave en este caso.
51 Cuando el aeromodelo vuela contra el viento fuerte, decide girar en una de las dos direcciones. Ilustración 53: Comportamiento volando frente al viento Este giro provoca un cambio en la trayectoria. Ilustración 54: Cambio del vector de trayectoria Este cambio en la trayectoria afecta al lazo de control de trayectoria puesto que ahora es, según el algoritmo que se ha programado, más eficiente girar en la otra dirección debido a que el ángulo es menor entre el vector de trayectoria y el de destino. El cambio de giro continúa hasta que el viento fuerte desaparece. Ilustración 55: Cambio del sentido de giro
52 6 Conclusiones El sistema P.A.T.O. es un sistema de bajo coste capaz de realizar tanto vuelos de búsqueda como vuelos experimentales para la carga en la bahía (del aeromodelo de ala fija que controla) de forma autónoma y sin necesidad de la intervención de un operador (aunque es necesaria su supervisión por motivos de seguridad). Pero como todo sistema de bajo coste, éste tiene sus limitaciones. 6.1 Autonomía En lo referente al “alcance y autonomía” del vehículo escogido para P.A.T.O., no dispone de demasiada autonomía debido a la poca energía que es capaz de cargar (en parte se debe a ser eléctrica su propulsión). 6.1.1 Turbofan o Turbojet Como propuesta de mejora se propone el diseño y fabricación de un motor turbojet o un turbofan. Esto presentaría nuevos problemas debido al movimiento del combustible dentro del tanque y al cambio de la masa total a lo largo del vuelo, así como el movimiento del centro de masas a lo largo del mismo. Ilustración 56: Esquema básico de un turbojet 6.1.2 Paneles solares Otra opción sería incorporar paneles solares en la parte superior de la superficie del aeromodelo. Se sugiere ponerlos en las alas y, mediante un convertidor DC-DC, recargar las baterías (principalmente las baterías de la propulsión, pero la aviónica debe recargarse también, así que esto desemboca en un diseño de BMS o Batery Management System muy interesante). Como desventaja esta solución sólo puede utilizarse durante el día y para operaciones nocturnas únicamente añade peso al aeromodelo.
53 6.2 Aeromodelo El aeromodelo escogido para llevar el prototipo P.A.T.O. es perfectamente capaz de llevar a cabo las tareas que se le han asignado: reconocimiento, búsqueda de objetivos, vuelo prediseñado para experimentos… 6.2.1 Estabilidad Esto se debe en gran parte a que las respuestas del aeromodelo son lentas, la velocidad no es muy alta y que el propio diseño del modelo se puede catalogar como estable. Esta estabilidad implica que el avión tiende a mantener un vuelo recto y nivelado por sí solo. Es similar a tener una canica sobre una superficie en forma de “U”: por mucho que la canica se mueva hacia uno de los extremos, en el instante que deje de intervenir la fuerza que la ha movido, ella misma volverá al centro de forma natural; también tiene un cierto coste energético mover la canica y es una buena forma de ilustrar la lentitud del sistema. Un ejemplo de un aeromodelo estable sería una avioneta. Sería interesante desarrollar una siguiente versión del sistema P.A.T.O. con un aeromodelo más críticamente estable y que por lo tanto sea más rápido en reacciones. Un símil del sistema críticamente estable sería una canica en una llanura. No requiere mucha energía mover la canica (por lo tanto velocidad de reacción más alta) y ésta no volvería al centro por sí sola. Un ejemplo sería un caza, como el F-22. No se aconseja el uso de P.A.T.O. en un aeromodelo inestable. Éste tendría reacciones excesivamente rápidas y presentaría muchos problemas para tan sólo mantener un vuelo recto y nivelado. Un símil sería una canica en una “U” invertida. No hay costo energético al desplazar la canica fuera del centro, pero costaría volver a él (y no volvería sola, más bien lo contrario). Como ilustración de este caso, la NASA necesitó 3 ordenadores en paralelo para realizar las correcciones necesarias para el vuelo recto y nivelado en su X-29, cosa que justifica este consejo sobradamente. 6.2.2 Sustentación, entrada en pérdida e hipersustentadores El coeficiente de sustentación máximo determina la carga alar del avión. Por lo tanto, desarrollar dispositivos hipersustentadores (Flaps y Slats) para aumentar el valor del coeficiente de sustentación máximo sería una ampliación interesante. Más detalles acerca del tema en [1]. 6.2.3 Velocidad La aeronave que P.A.T.O. gobierna está diseñada para vuelos lentos de cara a una mejor observación durante las operaciones de reconocimiento y búsqueda. Puede ser interesante rediseñar la aeronave para permitir vuelos más rápidos con la intención de ampliar las posibilidades de los vuelos con fines experimentales (experimentos en la bahía de carga), así como su alcance.
54 6.3 Posicionamiento global En principio P.A.T.O. dispone de un sistema G.N.S.S. conectado a la red GPS norteamericana para conocer su ubicación en el globo (así como la altura). 6.3.1 Precisión La precisión de una sola red de satélites G.N.S.S. es suficiente para la aeronavegación, pero puede ser insuficiente para un buen aterrizaje. Para mejorar la precisión se pueden usar 2 redes de satélites simultáneamente (con 2 receptores G.N.S.S.) a costa de un mayor consumo eléctrico. 6.3.2 Pérdida de la señal Cuando las antenas de base se encuentran en mantenimiento, la señal G.N.S.S. perderá precisión. Esto es inevitable, pero generalmente se tiene cobertura de hasta 3 antenas de base, por lo que no afectaría. Éstas sirven para corregir errores y conseguir una mayor precisión. Ilustración 57: Antena base GNSS En caso de tormenta eléctrica es muy probable que la tormenta produzca interferencias y por lo tanto se pierda señal G.N.S.S. Es una de las principales razones por las que no está permitido usar P.A.T.O. cuando exista la posibilidad de tormenta eléctrica. Pese a esto, un rediseño del conjunto antena + amplificador, del receptor GNSS puede ser interesante como mejora.
55 6.4 Algoritmos de control Los algoritmos de control de la aeronave cumplen su función según lo estipulado. Pese a ello, es posible mejorar ambos algoritmos de control. 6.4.1 Control Guiado El control guiado descrito en esta memoria realiza los giros tal y como se explica en los manuales de aeronavegación (ver [2] para más detalle). Las maniobras de ascenso y descenso, por el contrario, no siguen estas normas al pie de la letra; sería interesante modificar el algoritmo de control guiado para que haga estas maniobras según el manual de aeronavegación. 6.4.2 Control de trayectoria El control de trayectoria descrito aquí supone que la diferencia entre el vector de trayectoria y el de orientación son despreciables. Para la mayoría de trayectos este control es adecuado, pero, en caso de que tenga viento de frente a una velocidad mayor que su velocidad aerodinámica, el vector de trayectoria apuntaría en dirección opuesta. Dado el diseño del control, P.A.T.O. intentaría corregir su trayectoria girando a un lado. Este giro modificaría el vector de trayectoria lo suficiente como para que el lazo de control aplique un giro al otro lado. Esto provocaría un movimiento en zig-zag en dirección al destino escogido. Haciendo que la maniobra sea progresiva este zig-zag se vería minimizado, debido al continuo cambio de sentido. Modificar el control de trayectoria de esta manera, supondría un aumento de eficiencia para el proyecto. 6.4.3 Modo Repetidor Cuando P.A.T.O. recibe la señal de radio del objetivo que está buscando, sigue su trayectoria. Añadir una funcionalidad llamada Modo Repetidor es un paso lógico para este proyecto. En el momento en el que la bahía de carga (equipada con un radiorrepetidor para el objetivo a buscar) recibe la señal de baliza del objetivo, podrían hacerse dos cosas: La posición GNSS actual es almacenada como una variable nueva y la máquina de estados cambia a un estado nuevo que incluye esta posición. Esto implica que el aeromodelo girará en círculos intentando ir al punto una y otra vez. Tras recibir el ACK desde base, se continúa con la máquina de estados original (lista original de puntos GNSS). El radiorrepetidor recibe y desencripta la posición del objetivo. La posición desencriptada se entrega a P.A.T.O., que crea una nueva variable con esta nueva posición y repite el proceso descrito en el párrafo anterior.
56 Es difícil saber cuál de los dos métodos es mejor. En el caso de recibir un rebote de la señal momentáneamente, dar vueltas alrededor del punto actual no tendría mucho sentido; en el caso de que el objetivo esté cubierto por algo metálico (un tejado metálico por ejemplo), posicionarse sobre él llevaría al radiorrepetidor a un punto de sombra. El desarrollo de este modo de funcionamiento, así como la relación de compromiso entre los dos métodos (o incluso algún método alternativo) serán objeto de la próxima iteración de P.A.T.O. 6.5 Lazo de control de velocidad aerodinámica El control de motor actualmente en este aeromodelo consiste en un “todo o nada”. Añadir un control de velocidad que sea capaz de mantener la velocidad aerodinámica del aparato, e incluso de asegurar una velocidad aerodinámica mínima para mantener la sustentación del avión, es algo a tener en cuenta como ampliación del sistema. Como instrumento de medición de la velocidad aerodinámica se utilizaría un tubo de Pitot y como motor se utilizaría un motor síncrono o un brushless (siendo este segundo el más lógico por su bajo coste). Para gobernar el motor se propone el integrado DRV8343-Q1 de Texas Instruments, que permite incluso el control de motores brushless con sensores Hall. Ilustración 58: Módulo Arduino compatible BMP280 (Sensor Barométrico) Para el tubo de Pitot se propone el uso de los sensores barométrico BMP280. Es posible realizar un pequeño montaje en el que se midan tanto la presión total (con uno de los sensores) como la presión estática (con otro). Teniendo ambas, se puede obtener la presión dinámica y desde ahí calcular la velocidad aerodinámica del avión. En caso de querer incorporar el sensor a una PCB propia el siguiente esquema puede servir como referencia. Ilustración 59: Esquemático BME280 (Chip de BMP280)
63 Grupo Señal I/O Descripción Tensión CAN CAN_RX I CAN Bus receive data. 3.3 V CAN_TX O CAN Bus transmit data. 3.3 V UART UART_RX1 I UART receive data 3.3 V UART_TX1 O UART transmit data 3.3 V UART_RTS1 O UART ready to send 3.3 V UART_CTS1 I UART clear to send 3.3 V UART_RX2 I 2nd UART receive data 3.3 V UART_TX2 O 2nd UART transmit data 3.3 V
64 Grupo Señal I/O Descripción Tensión I2C I2C_SCL I/O I2C clock. Open drain with pullup on carrier board 3.3 V I2C_SDA I/O I2C data. Open drain with pullup on carrier board. 3.3 V I2C_INT# I Interrupt notification from carrier board to processor. Open drain with pullup on carrier board. Active LOW. 3.3 V I2C_SCL1 I/O 2nd I2C clock. Open drain with pullup on carrier board. 3.3 V I”C_SDA1 I/O 2nd I2C data. Open drain with pullup on carrier board. 3.3 V
65 Grupo Señal I/O Descripción Tensión SPI SPI_COPI O SPI Controller Output/Peripheral Input 3.3 V SPI_CIPO(I) I SPI Controller Input/Peripheral Output 3.3 V SPI_SCK O SPI Clock 3.3 V SPI_CS# O SPI Chip Select. Active LOW. Can be routed to GPIO if hardware CS is unused. 3.3 V SPI/SDIO SPI_SCK1/SDIO_CLK O 2nd SPI Cloc. Secondary use is SDIO Clock 3.3 V SPI_COPI1/SDIO_CMD I/O 2nd SPI Controller Output/Peripheral Input. Secondary use is SDIO command interface. 3.3 V SPI_CIPO1/SDIO_DATA0 I/O 2nd SPI Peripheral Input/Controller Output. Secondary use is SDIO data Exchange bit 0. 3.3 V SDIO_DATA1 I/O SDIO data Exchange bit 0. 3.3 V SDIO_DATA2 I/O SDIO data Exchange bit 0. 3.3 V SPI_CS1/SDIO_DATA3 I/O 2nd SPI Chip Select. Secondary use is SDIO data Exchange bit 0. 3.3 V
66 Grupo Señal I/O Descripción Tensión Audio AUD_MCLK O Audio master clock 3.3 V AUD_OUT/PCM_OUT/I2S_OUT/CAM_MCLK O Audio data output. PCM synchronus data output. I2S serial data out. Camera master clock 3.3 V AUD_IN/PCM_IN/I2S_IN/CAM_PCLK I Audio data input. PCM synchronus data input. I2S serial data in. Camera peripheral clock 3.3 V AUD_LRCLK/PCM_SYNC/I2S_WS/PDM_DATA I/O Audio left/right clock. PCM Synchronous data SYN. I2S Word select. PDM data. 3.3 V AUD_BCLK/PCM_CLK/I2S_CLK/PDM_CLK O Audio bit clock. PCM clock. I2S continous serial clock. PDM clock. 3.3 V SWD SWDIO I/O Serial Wire Debug I/O. Connect if processor board suppors SWD. Can be left NC. 3.3 V SWDCK I Serial Wire Debug clock. Connect if processor board supports SWD. Can be left NC. 3.3 V
67 Grupo Señal I/O Descripción Tensión ADC A0 I Analog to digitial converter 0. Amplify the analog signal as needed to enable full 0-3.3V range. 3.3 V A1 I Analog to digital converter 1. Amplify the analog signal to enable full 0-3.3V range. 3.3 V PWM PWM0 O Pulse Width Modulated Output 0 3.3 V PWM1 O Pulse Width Modulated Output 1 3.3 V Digital D0 I/O General digital input/output pin. 3.3 V D1/CAM_TRIG I/O General digital input/output pin. Camera Trigger. 3.3 V
68 Grupo Señal I/O Descripción Tensión General/Bus G0/BUS0 I/O General purpose pins. Any unusd processor pins should be assigned to Gx with ADC + PWM capable pins given priority (0, 1, 2, etc.) positions. The intent is to guarantee PWM ADC and Digital Pin functionality on respective ADC/PWM/Digital pins. Gx pins do not guarantee ADC/PWM function. Alternativ use is pins can support a fast read/write 8bit or 4-bit 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 General purpose pin 3.3 V G9/ADC_D-/CAM_HSYNC I/O Differential ADC input if available. Camera horizontal syn 3.3 V G10/ADC_D+/CAM_VSYNC I/O Fieferential ADC input if available. Camera vertical sync. 3.3 V G11/SWO I/O General purpose pin. Serial Write Output 3.3 V
69 8.2 Filtro de orientación 8.2.1 Representación por “Cuaterniones” La siguiente información se ha obtenido de [7]. Un cuaternión es un número complejo de cuatro dimensiones que puede utilizarse para representar la orientación de un cuerpo fijo o el marco de coordenadas en un espacio tridimensional. Una orientación arbitraria del marco B relativo al marco A se puede conseguir mediante una rotación del ángulo θ sobre el eje Ar definido en el marco A. Esto se representa en la siguiente figura, donde los vectores mutuamente ortogonales xA, yA y zA y xB, yB y zB definen los principales ejes de coordenadas de los marcos A y B. Ilustración 60: Representación gráfica de los marcos A y B El cuaternión que describe la rotación , se define en la ecuación: [ ] [ ] Donde rX, ry, rz definen los componentes del vector unitario Ar en x, y, z respectivamente sobre el marco A. El sistema de superíndice y subíndice utilizado se usa para indicar el marco de referencia como superíndice y el subíndice como el marco que está siendo descrito. Por ejemplo describe la orientación del marco B relativo al marco A y Ar es un vector descrito en el marco A. La aritmética de cuaterniones generalmente requiere que el cuaternión que describe una orientación sea antes normalizado. Por lo tanto, es lo convencional que todos los cuaterniones tengan longitud unidad.
70 El conjugado de un cuaternión, indicado por “*”, se puede usar para intercambiar marcos relativos descritos por una orientación. Por ejemplo, es el conjugado de y describe la orientación de A relativa al marco B. [ ] El producto de cuaterniones, indicado por ⊗, se puede usar para definir orientaciones compuestas. Por ejemplo, para dos orientaciones descritas por , la orientación compuesta se define como: ⊗ Para dos cuaterniones a y b, el producto de cuaternión se puede determinar usando la regla de Hamilton y se define como se puede ver en la siguiente ecuación. Un producto de cuaternión no es conmutativo, por lo que a ⊗ b no es igual que b ⊗ a. ⊗b [ ]⊗[ ] [ ] Un vector tridimensional puede ser rotado por un cuaternión utilizando la relación descrita en la siguiente ecuación. Av y Bv son el mismo vector descrito en el marco A y en el marco B respectivamente, donde cada vector contiene un cero como primer elemento para convertirlo en un vector de 4 filas. ⊗ ⊗ La orientación descrita por se puede representar como una matriz de rotación definida por: [ ] Los ángulos θ Φ en la llamada secuencia aeroespacial describen la orientaci n del marco B por las rotaciones secuenciales en alineaci n con el marco A de alrededor de zB, θ sobre yB y Φ sobre xB. La representación por ángulos de Euler de se define por: atan θ Φ atan
71 8.2.2 Orientación por ratio angular Un giróscopo de 3 ejes mide el ratio angular sobre los ejes x, y, z nombrados ꙍx, ꙍy, ꙍz respectivamente. Si estos parámetros (en radianes-1) se distribuyen en el vector sꙍ, la derivada del cuaternión que describe el ratio de cambio de orientación del marco terrestre relativo al marco del sensor se puede calcular: [ ] ⊗ La orientación del marco terrestre relativo al marco del sensor en el tiempo t, , puede ser computado integrando numéricamente la derivada de cuaternión siempre que las condiciones iniciales sean conocidas. En estas ecuaciones, Sꙍt es el ratio angular medido en el instante t, ∆t es el periodo de muestreo y es la estimación de la orientación anterior. El subíndice ꙍ indica que el cuaternión es calculado a partir de ratios angulares. ⊗ t 8.2.3 Orientación por observación vectorial Un acelerómetro de 3 ejes mide la magnitud y dirección del campo gravitatorio en el marco del sensor compuesto por aceleraciones lineales. De forma parecida un magnetómetro de 3 ejes mide la dirección y magnitud del campo magnético de la tierra respecto al marco del sensor compuesto por el flujo magnético local y distorsiones. En el contexto de un filtro de orientación se asume que un acelerómetro sólo medirá la gravedad y un magnetómetro sólo medirá el campo magnético terrestre. Si la dirección de un campo terrestre es conocida en el marco terrestre, una medida de la dirección del campo en el marco del sensor permitirá el cálculo de una orientación del marco del sensor relativa al campo terrestre. Sin embargo, para cualquier medida dada no habrá una única solución de orientación, en su lugar habrá infinitas soluciones representadas por todas las orientaciones conseguidas debidas a la rotación de la auténtica orientación alrededor de un eje paralelo al campo.
72 Una representación vía cuaterniones requiere encontrar una solución completa. Esto se puede conseguir a través de la formulación de un problema de optimización, donde la orientación de un sensor es alineada con una dirección predefinida al campo del marco terrestre Ed, con la dirección del marco del sensor Ss, utilizando la operación de rotación anteriormente descrita. Por lo tanto se puede encontrar como la solución a: Donde la ecuación siguiente define la función objeto: ( ) ⊗ ⊗ Los componentes de cada vector se describen como: [ ] [ ] [ ] Existen varios algoritmos de optimización pero el algoritmo de gradiente descendente es uno de los más simples de implementar y computar. La siguiente ecuación describe el algoritmo para “n” iteraciones en una estimación de la orientación de y un tamaño de paso μ. ( ) ‖ ( )‖ La siguiente ecuación computa el gradiente de la solución de superficie definida por la función objetivo y su Jacobiano. ( ) ( ) ( ) ( ) [ ( ) ( ) ( ) ] ( ) [ ] Estas ecuaciones describen de forma general el algoritmo aplicable a un campo predefinido en cualquier dirección. Sin embargo, si es posible asumir que la dirección del campo sólo tiene componentes en 1 ó 2 de los ejes principales del marco de orientación global, las ecuaciones se simplifican.
79 El mensaje xbus reducido queda tal que (en orden de recepción): 1. MID (1 byte) --> Identificativo de mensaje. 2. LEN (1 byte) --> Longitud en bytes de los datos. 3. DataID (2 bytes) --> Identificativo del dato enviado. 4. Data_LEN (1 byte) --> Longitud del dato enviado (referente al DataID) 5. Datos (Data_LEN bytes) --> Datos enviados [...] --> (Puede enviar más datos y se repetiría desde el punto 3) 6. CheckSum (1 byte) --> CheckSum del mensaje suponiendo el BusID = 0xFF. Ilustración 63: Estructura de la trama SPI de MTi 1 - series 8.3.2.2 Opcode El primer byte que llega vía SPI es el "opcode". Según el opcode el formato del mensaje varía. Por lo tanto, se cataloga el mensaje primero según el opcode: - OPCODE 0x01 --> Lectura --> Definido por opcode - OPCODE 0x02 --> Escritura --> Definido por opcode - OPCODE 0x03 --> Escritura --> xbus reducido - OPCODE 0x04 --> Lectura --> Definido por opcode - OPCODE 0x05 --> Lectura --> xbus reducido - OPCODE 0x06 --> Lectura --> xbus reducido 8.3.2.3 Configuración SPI Modo 4 - hilos SPI: - Chip Select / Slave Select (SPI_nCS) - Serial Clock (SPI_SCK) - Master data In, Slave data Out (SPI_MISO) - Master data Out, Slave data In (SPI_MOSI) El módulo utiliza el modo 3 de SPI: Captura datos en el flanco ascendente de reloj y propaga los datos en el flanco descendente. (CPOL = 1 y CPHA=1). La información se envía MSB primero. El módulo utiliza un formato de 8 bits.
80 Ilustración 64: Funcionamiento SPI de MTi 1 - series - T1: Tiempo entre flanco descendente de Slave Select y primer paquete enviado: Mín 4 us - T2: Tiempo de envío de un byte: Mín 4 us - T3: Tiempo para envío consecutivo: Mín 3 us - Bitrate máximo del SPI: 2 Mbits/s 8.3.2.4 Funciones de la librería desarrollada xsens_SPI_init(struct spi_device *mti_spi) Primero reinicia la MTi 1-series accionando el pin reset durante un tiempo determinado. Realiza una comunicación con la IMU vía SPI en la que se envía el OPCODE "PIPE_STATUS" de modo que devuelva 2 uint16 que se leen, pero que por ahora quedan en el olvido. Lo ideal sería esperar recibir confirmación de que la IMU se ha reiniciado (posible mejora). Una vez se ha terminado de recibir la información esperada en el bus SPI la función termina. Necesita como variables: - Puntero a la estructura del spi_device que vaya a usar. xsens_SPI_actualizarMedidas(struct spi_device *mti_spi) Llama a la función "xbus_spi_read()" y le da información del bus spi_device que se está utilizando y el pin SS que necesitará para realizar su función. Es posible ampliar más adelante esta función para que se encargue de actualizar estructuras de datos donde se almacene la información de los sensores. Necesita como variables: - Puntero a la estructura del spi_device que vaya a usar. xbus_spi_read(struct spi_device *mti_spi) Realiza llamadas a las funciones "xbus_spi_readPipeStatus()", "xbus_spi_readPipeNotif()", "xbus_spi_parseData()" y les da la información que necesitan para funcionar. xbus_spi_readPipeStatus(struct spi_device *mti_spi) Solicita vía SPI el tamaño de la infromación preparada para enviar vía SPI por parte de la MTi 1-series, tanto de mensajes de notificación como de mensajes de mediciones. Guarda los valores en los arrays notificationSize y measurementSize. Necesita como variables: - Puntero a la estructura del spi_device que vaya a usar.
81 xbus_spi_readPipeNotif(struct spi_device *mti_spi) Solicita y recibe los mensajes de notificación preparados de la MTi 1-series. Necesita como variables: - Puntero a la estructura del spi_device que vaya a usar. xbus_spi_readPipeMeas(struct spi_device *mti_spi) Solicita y recibe los mensajes de mediciones preparados de la MTi 1-series. Necesita como variables: - Puntero a la estructura del spi_device que vaya a usar. xbus_spi_dataswapendian(uint8_t* data, uint8_t length) Cambia el orden de los bytes (4 bytes cada vez) del array que se le hayan enviado. Es utilizado por la función "xbus_spi_parseData". Necesita como variables: - Puntero al array de datos (En este caso del dato que se quiere leer, no el array completo) - Tamaño del dato a leer del array. xbus_spi_parseData(uint8_t* data, uint8_t datalength) Realiza el parseo del mensaje de mediciones recibido. La función de momento guarda los datos recibidos en sus variables correspondientes, pero sería más conveniente que estas variables formasen parte de una estructura de datos ya predefinida (que todo esté más ordenado). Necesita como variables: - Puntero al array de datos (Mensaje leído del measurement pipe) - Tamaño del array de datos (Tamaño del mensaje recibido por measurement pipe)
82 Ilustración 65: Diagrama de flujo del funcionamiento del Parser de la trama SPI
83 8.4 Microcontrolador ATMEGA2560 8.4.1 Diagrama de bloques del microcontrolador Ilustración 66:Diagrama de bloques del Microcontrolador
84 8.4.2 Periférico SPI Ilustración 67: Diagrama de bloques del periférico SPI del Microcontrolador
85 Funcionalidad /SS y modos de funcionamiento: Ilustración 68: Funcionalidad del pin /SS y modos de funcionamiento
86 Registros de control y estado: Ilustración 69: Registros de control de SPI
87 Ilustración 70: Registros de control y estado
88 8.4.3 Periférico USART Formato de las tramas: Ilustración 71: Formato de las tramas
95 Registros TWI:
96
97
98
99 8.5 Módulo receptor GNSS Descripción del formato de las tramas NMEA:
100 Lista de mensajes NMEA:
101 Descripción del mensaje NMEA que activa o desactiva el envío de cada mensaje:
102 Se utilizan dos tramas GNSS para obtener los datos necesarios para el funcionamiento del sistema: GPGGA y GPMRC
103
104 Descripción del formato de los mensajes UBX:
111
112 8.6 MARG Mapa de registros del MPU 9250:
113
114
115 Registros de configuración del acelerómetro y giróscopo:
116 Registros de configuración del ByPass. Permite acceder a través del bus I2C a los registros del magnetómetro incluidos en el encapsulado:
117 Registros de lectura de acelerómetro y giróscopo:
118
119
120
127 8.7 APC220 8.7.1 Pinout del Módulo
128 8.7.2 Parámetros de configuración
129 8.7.3 Especificaciones técnicas del APC220
Como implementar un PID con Microcontroladores 1 Abstracto— Este documento trata el tema de los controladores PID en microcontroladores. Se empieza con una breve resumen de la teoría detrás del control automático y los controladores PID para terminar con la programación en microcontroladores de los mismos. En la primera sección se hace una breve introducción de lo que es el control automático. Después, en la segunda sección se explican los fundamentos de la teoría de control moderna. En la tercera y cuarta sección se verá una aproximación intuitiva a lo que es un controlador PID y su tunning. En la quinta sección se verá la forma de implementar un PID básico así como un método eficaz de corregir uno de sus defectos más recurrentes, el windup. En la sexta sección se ven las conclusiones del documento y finalmente, en la séptima sección se ven una serie de referencias bibliográficas del tema tratado en este documento. I. INTRODUCCIÓN l control automático es una rama de la ingeniería que se ocupa del control de un proceso en un estado determinado; por ejemplo, mantener la temperatura de una caldera, el rumbo de un cohete o la velocidad de un motor. Los controladores automáticos comparan el valor de salida de una planta con el valor deseado (consigna) y produce una señal de control que reduce la desviación a cero, o en su defecto a un valor pequeño. La forma en que este control produce la señal de control recibe el nombre de acción de control. Es posible clasificar en diferentes tipos de controladores de acuerdo a su acción de control: a) Controladores Todo o Nada b) Controladores Proporcionales (P) c) Controladores Integrales (I) d) Controladores Proporcionales e Integrales (PI) e) Controladores Proporcionales y derivativos (PD) f) Controladores Proporcionales, derivativos e integrales (PID) En este documento se hará especial énfasis en los controladores PID. II. INTRODUCCIÓN A TEORÍA DE CONTROL Aquí se intentará hacer algo que no es tan frecuente de encontrar, dar una visión intuitiva de la teoría de control y de la motivación que hay detrás, de forma que sea fácilmente comprensible, y sin tener que usar transformadas de Laplace, lugares de las raíces, diagramas de Bode, ni teoremas de Nyquist. A. El sistema Se denomina sistema a una representación simplificada de la realidad que es abstraída para poder trabajar con él. Normalmente se representa una parte de la realidad que aislamos. Se puede considerar sistema a casi cualquier cosa. La presión de una caldera, el sistema de climatización de un coche, el equipo de bombeo de un depósito, los controles de un avión, la propulsión de una nave espacial, un robot... hasta el sistema digestivo de un pato. Un sistema, al que normalmente se le hará referencia como planta, tiene una o varias entradas que representan acciones sobre las que se puede actuar. También tiene una o varias salidas que son los parámetros que se pueden observar y medir. En general un sistema tiene muchas entradas y salidas, pero normalmente se van a simplificar a las más importantes. La planta, además, tiene un cierto comportamiento. Es decir, realizar una acción tendrá una determinada consecuencia sobre sus salidas. Por supuesto, el comportamiento puede ser todo lo complicado que se pueda imaginar. Puede ser no lineal, tener inercias, incluso retrasos puros (un tiempo entre que se aplica la acción y el sistema actúa), efectos entre varias entradas. El comportamiento de la planta también puede ser abstraído y modelado de forma simplificada, en lo que se conoce como función de trasferencia. Y se puede representar, por ejemplo, mediante una ecuación, un diagrama de bloques, una representación gráfica o frecuencial. Cómo implementar un PID con Microcontroladores A. Elgezabal Núñez, BiSKY Team, Bilbao E
Como implementar un PID con Microcontroladores 2 Además la planta puede tener perturbaciones, es decir, modificaciones en el entorno que modifican su comportamiento. Por ejemplo, en el edificio la temperatura exterior va a cambiar, en el coche se puede subir o bajar una cuesta, y en el embalse se puede poner a llover. En resumen, un sistema es un modelo de una porción del universo, que tiene entradas y salidas, cuya relación entre éstas sigue un determinado comportamiento y que está sometido a posibles perturbaciones. . Y esto se suele representar así: Figure 1: Representación de un sistema B. El controlador El controlador es un elemento que se pone antes de las entradas de nuestra planta para actuar sobre ellas con objeto de conseguir que la salida tenga unas determinadas características. Por ejemplo, si se quiere controlar la temperatura de un edificio actuando sobre la caldera, la velocidad del coche actuando sobre el motor, o el nivel del depósito actuando sobre el bombeo. Lógicamente, la entrada sobre la que se va a actuar debe tener una influencia en la salida que se quiere controlar. Si se está controlando una variable que es totalmente independiente de lo que se está midiendo, no se conseguirá nada. El controlador puede ser desde una persona, hasta un sistema analógico, o un sistema digital. En sistemas automáticos lógicamente se hace referencia a algún tipo de máquina controlada por el sistema de forma autónoma sin intervención manual. El controlador, representado junto al sistema, quedaría así. Figure 2: Representación de Controlador y sistema C. Características a la salida Lo más evidente es que la salida tenga un valor determinado que se denomina consigna. Por ejemplo, se quiere que la temperatura sea de 24ºC, que el coche vaya a 80km/h, o que el nivel del agua de un embalse sea 175m. Por supuesto, la consigna no tiene que ser la misma siempre. Se puede querer que la temperatura sea 17ºC durante la noche y 24ºC durante el día, que el coche pase a 50km/h al pasar por una travesía, o querer vaciar a 160m embalse porque tengo que suministrar agua potable a la red. Pero, además de la consigna, hay otras características deseadas para la salida. En general, se va a querer que la salida sea estable a largo plazo, que converja al valor de la consigna, que el tiempo de respuesta sea rápido, que no oscile, que no sobrepase el valor de consigna en ningún momento. No se va a poder tenerlas todas a la vez, así que se tiene que llegar a un compromiso entre todas. Aunque, llegado el caso puede, que se esté dispuesto a sacrificar unas respecto a otras. Por ejemplo, quizás no importa que la temperatura de un edificio alcance brevemente 24.5ºC, si así el tiempo de respuesta es menor. Pero si a 180m de agua el embalse se rompe, inundando un pueblo y matando a cientos de personas, quizás no sea tolerable el desbordamiento y se prefiera que suba el nivel lento pero seguro. Por tanto, las características deseadas para la salida dependen totalmente del sistema y de lo que se quiera hacer, y por tanto el controlador que se tiene que emplear. D. Lazo Cerrado Para que un controlador funcione tiene que tener realimentación. Básicamente el controlador tiene que poder monitorizar la salida que intenta controlar directa o indirectamente. Sin realimentación, un controlador no es un controlador. Está claro que para controlar una salida hay que poder medirla. Así que se coge la salida y se compara con la consigna para ver el error que hay. Se emplea este error como entrada del controlador. Esto se expresa así.
Como implementar un PID con Microcontroladores 3 Figure 3 Representación de un Lazo Cerrado (L.C.) El error entre la medición y la consigna puede ser bien porque aún no se ha conseguido que la salida alcance a la consigna, o porque se ha cambiado la consigna. Con realimentación el sistema global (controlador + planta) se comporta de forma totalmente distinta. La salida puede desde tender obedientemente a la consigna, a ponerse a oscilar como una loca hasta que rompa algo. Y la diferencia entre una salida (éxito absoluto en el sistema de control) y la otra (rotura, despido, y posible destrucción del universo) depende totalmente del control que se diseñe. III. APROXIMACIÓN INTUITIVA AL CONTROLADOR PID En este capítulo se verá el controlador PID, uno de los controladores más extendidos por su sencillez y por ser capaz de conseguir un buen comportamiento en una gran variedad de situaciones. En el capítulo anterior se vieron algunas generalidades y definiciones sobre teoría de sistemas. Por supuesto, no se va a entrar en profundidad en los detalles del PID, dado que es tema muy extenso (e interesante). Si existe interés se puede consultar la abundante documentación disponible. En este capítulo se intenta dar una visión intuitiva del controlador y de la motivación que explica su comportamiento. Sin usar ecuaciones, ni matemáticas. De hecho, es una de las cosas geniales del PID, que no es necesario saber en detalle cómo funciona, para hacer que funcione. A. ¿Qué es el PID? El controlador PID es uno de los más empleados en la industria para el control de sistemas realimentados. Algunas de sus fortalezas son su sencillez y que es capaz de dar un buen comportamiento en una gran variedad de situaciones sin necesidad de conocer con detalle la planta a controlar. El controlador PID es conocido desde hace tiempo. Sus primeros usos datan de 1911, y su primer análisis teórico de 1922 de la mano de Nicolas Minorsky. En esos tiempos, el control PID era exclusivamente analógico. Sin embargo, resulta fácil implementar en programación un PID digital y los cálculos que requiere son sencillos y eficientes. Además, es "relativamente sencillo" extrapolar la teoría de los PID analógicos a su equivalente digital. Pese a su popularidad, hay que decir que actualmente el PID no es el mejor controlador disponible. Pero en la mayoría de los casos es más que suficiente. Por otro lado, muchos de los controladores más "modernos" no dejan de ser versiones mejoradas de un PID, como por ejemplo las distintas familias basadas en PID con parámetros adaptativos. B. Funcionamiento del PID El algoritmo PID (proporcional, integral, derivativo) está formado por la suma de tres componentes, Proporcional, Integral, y Derivativo. Matemáticamente, un controlador PID tiene la siguiente formulación. Cada componente del PID es "independiente" de los demás, en el sentido de que cada uno calcula una salida de lo que "para él" debería hacer para obtener la respuesta adecuada. Los tres componentes se suman para dar la salida del controlador. Cada uno cumple una cierta función y mejoran cierta parte de la respuesta. Y cuando los tres componentes trabajan juntos, en la proporción adecuada, consiguen un gran comportamiento. Figure 4: PID en un Lazo Cerrado Cada componente tiene un parámetro Kp, Ki y Kd, respectivamente. Estos parámetros indican la ponderación (o "la fuerza") que tiene en el resultado final. Que la respuesta del PID sea buena, cosas rotas, muerte y destrucción, reside en el ajuste correcto de estos tres parámetros.
Como implementar un PID con Microcontroladores 4 Y aquí viene la parte "graciosa" del PID. En los tres parámetros, si se pone un valor muy bajo no se va a notar el efecto del componente en la salida. Y si se pone demasiado alto… pues eso, muerte, destrucción, cosas rotas, etc. Es más, en la respuesta global del controlador los tres componentes trabajan juntos e influyen uno sobre otros, por lo que no vale con ajustar cada uno de los parámetros de forma independiente. Existe una cierta "zona" dentro de los tres parámetros, donde el comportamiento es más o menos bueno. Lógicamente, queda claro que la dificultad (que tampoco es para tanto) de un PID es ajustar los parámetros K, Ki y Kd, para que el comportamiento sea bueno. Como se ha explicado, el controlador PID se basa en tres componentes, PID. Su fortaleza reside en el papel que cada uno de estos componentes tiene en la respuesta. A modo resumen: El componente proporcional reacciona al presente. El componente integral reacciona al pasado, y aporta "memoria" al controlador. El componente derivado reacciona al futuro, y aporta "predicción" al controlador C. Componente Proporcional – P 1) Motivación La motivación del componente proporcional seguramente sea el más intuitivo de explicar. Si se entra en la habitación y ven 12ºC, parece lógico que hay que darle más a la calefacción que si pone 23ºC. Pues esa es la motivación del componente proporcional, que tiene sentido que se aplique más acción cuanto más lejos está del valor deseado, y viceversa. 2) Formulación El componente proporcional se calcula simplemente como un factor K multiplicado por el error (diferencia entre consigna y valor real). 3) Comportamiento El factor proporcional tiene una gran influencia en la velocidad de respuesta del sistema. Si se tiene una K pequeña, el sistema va a tardar mucho en alcanzar la consigna, porque le está aplicando poco al actuador. Si se aumenta, se consigue disminuir el tiempo de respuesta. Pero si aumenta la K demasiado se puede sobrepasar la consigna u oscilar. Si se tiene una K demasiado grande, se le va a dar demasiado a la calefacción. Así al ver 18ºC, se le da a la calefacción y el siguiente valor es 25. se ha pasado. Baja la calefacción, 21, ahora se ha pasado hacia abajo. Sube la calefacción, 23. Bien, ya lo tiene. Pero si lleva una K aún más grande, podría pasar que al ver 18º le dé MUCHO a la calefacción, y lo siguiente que se vea podría ser 32ºC. ¡Ostras, se he pasado una barbaridad, BAJA LA CALEFACCIÓN! 14º ¡OSTRAS SUBE SUBE! 45º... 8º... 56º... Acaba de hacer que el sistema oscile y sea inestable. Otra característica es que, en general, el componente proporcional no elimina por completo el error a largo plazo. En este ejemplo, supongamos que a 22ºC y una “posición” de la calefacción determinada, la energía que aportamos al edificio es exactamente la que necesita el edificio para mantener la temperatura. Se habría conseguido que la temperatura sea estable, pero como la “posición” de la calefacción viene dada por el error (que se ha estabilizado) nunca se podrá subir los 2ºC que faltan. D. Componente Integral – I 1) Motivación La función del componente integral quizás sea el más complicado de explicar dentro del PID. Imaginar que se está en la sala, y hay 22ºC. El proporcional dice que se ponga la calefacción en una “posición” y el controlador lo pone ahí. Pasa un minuto, pasa otro, pasa otro... y eso sigue en 22ºC, 2ºC por debajo de los que se quiere. El componente integral es el que dice... ¿oye chicos, igual hay que subir un poco la calefacción no? Porque llevamos un buen rato en 22ºC sin mover la calefacción, y eso no tiene pinta de que vaya a irse. Pues esa voz con tanto sentido, que reacciona a la memoria del error pasado, es el componente integral. 2) Formulación El componente integral aplica una acción que es proporcional a la integral del error a lo largo del tiempo. Equivalentemente, responde proporcionalmente a la suma de todos los errores anteriores. Gráficamente corresponde con el área encerrado bajo la curva del error, que también es lo mismo que el área entre la salida y la consigna. En el campo discreto, la integral se sustituye por un método discreto para su cálculo, como el cálculo mediante rectángulos o trapecios.
Como implementar un PID con Microcontroladores 5 3) Comportamiento El componente integral permite al controlador eliminar por completo el error a largo plazo. Sin embargo, si Ki es muy pequeño, el sistema tardará mucho en eliminar el error. Por otro lado, hay que tener en cuenta que el término integral únicamente responde al área entre la salida y la consigna. Una consecuencia de ello es que si hemos acumulado (por ejemplo) un error por estar por debajo de la consigna, la única forma que tiene el término integral de compensarlo es estar un tiempo por encima. Y efectivamente, el componente integral tiene tendencia a sobrepasar y oscilar, más incluso que el término proporcional. E. Componente Derivativo – D 1) Motivación El componente derivado también es sencillo de explicar. Suponiendo que se está tranquilamente con la temperatura a la consigna a 24ºC. ¡Todo perfecto! La temperatura baja a 23ºC, luego a 22ºC, y se sube un poquito la palanca. Todo va tal y como se espera. Ahora imaginando que, cuando se estaba a 24ºC la temperatura baja a 18ºC de golpe. Ostras, eso es una buena bajada. Se subes la calefacción. Marca 12ºC... Han abierto una ventana, pero ¡La temperatura sigue bajando muy rápido! Siguiente valor 2ºC. Ésa es la función del componente derivado, reaccionar ante variaciones del error (por cambio de consigna o de la variable). Porque no es lo mismo estar a 12ºC y subiendo lentamente, que si la temperatura se está desplomando a toda velocidad. Es decir, el término derivativo responde a la velocidad de cambio del error. 2) Formulación El componente derivado se calcula de forma proporcional a la derivada del error respecto del tiempo en el momento presente. Equivalentemente, responde proporcionalmente a la diferencia entre el error actual y el error en el instante anterior. En el campo discreto, la derivada se sustituye por la diferencia entre el error actual y el error anterior, dividido por el tiempo de muestreo (o se obvia esta división por completo y se incluye en el coeficiente Kd, si el tiempo de muestreo es constante). 3) Comportamiento El componente derivado mejora la respuesta general de muchos sistemas para valores de Kd moderados. Sin embargo, si se excede de Kd se verá que aparece una falta de "suavidad" en la respuesta, y otros comportamientos "raros". Además, el componente derivado responde muy mal al ruido de la medición. El ruido, una variación de alta frecuencia, supone variaciones muy rápidas, aunque sean de pequeña amplitud. Estas variaciones son amplificadas por el componente derivado. Y, por último, el componente derivado a veces es un poco "bestia" porque pide acciones muy grandes. Imaginar, por ejemplo, un cambio instantáneo en la consigna. El componente derivado demandaría una acción infinita que el accionador no podría satisfacer. Como el accionador no podría dar la acción que solicita el controlador, se tendrían desviaciones respecto a lo calculado, o incluso se podría dañar el accionador. IV. TUNNING DEL PID En función del valor que se ponga a los parámetros, se obtendrá una buena respuesta, o una respuesta lenta, u oscilante, o incluso ... muerte, destrucción. Para hacerlo más complicado, el efecto de los factores en la respuesta no es independiente. Su aportación se mezcla y entremezcla. Por lo que no se pueden ajustar los parámetros de "uno a uno". El éxito del controlador depende totalmente del ajuste. Para ello, es importante que se conozcan las características y efectos de cada factor en la respuesta del sistema. A. Resumen de los efectos del PID 1) Componente Proporcional Poco K, respuesta lenta. Mucho K, desbordamiento, oscilación, e incluso inestabilidad. No consigue eliminar el error en estacionario. 2) Componente Integral Elimina el error estacionario Demasiado Ki, oscilación e inestabilidad 3) Componente Derivativa Mejora el comportamiento general Demasiado Kd, comportamiento "raro" en la salida Muy sensible al ruido Muy sensible a cambios bruscos en el error (perturbaciones o cambio de consigna)
Como implementar un PID con Microcontroladores 6 B. Ajustando el PID Existen muchas formas de ajustar un PID, algunas más o menos teóricas. 1) Caracterización de la planta Consiste en determinar la función de transferencia de la planta. Prácticas habituales son aplicar una entrada escalón o frecuencial, y analizar las características de la salida. Después se puede operar matemáticamente para realizar un cálculo exacto del controlador para la respuesta requerida. 2) Aplicación de reglas de sintonización De forma similar, consiste en aplicar una determinada entrada al sistema, y medir la salida. A continuación, se aplican una de las muchas reglas de sintonización (ejemplo famoso, reglas Ziegler-Nichols) para obtener los parámetros del PID. 3) Auto Tunning En similar al anterior, pero realizado de forma automática por el controlador. Los reguladores PID han avanzado mucho y, en la actualidad, muchos tienen estupendos algoritmos de auto ajuste. Precisamente, esta capacidad de auto ajuste es uno de los motivos del éxito comercial de los controladores PID. 4) Ajuste manual (Heurístico) Finalmente, está el ajuste "a mano". Que, aunque suene fatal, es una opción muy válida y muy popular. En muchas ocasiones, un operador humano puede conseguir un ajuste tan bueno (o incluso superior) a un Auto tuning genérico. C. Ajuste manual del PID Para ajustar el PID manualmente se debe poder actuar sobre la entrada del sistema, sobre los parámetros del PID, y visualizar la respuesta. Y aquí, "visualizar" no significa necesariamente poner un osciloscopio. En el caso de un motor, por ejemplo, se puede "ver" la respuesta del sistema simplemente mirando cómo gira (o cómo suena). Hay dos métodos principalmente: 1) Ajuste Proporcional/Integral/Derivativo Ajustar K hasta que el sistema sobre pase o empiece a sobre oscilar. Ajustar Ki hasta eliminar error estacionario. Aumentar Kd hasta que empiezan a pasar cosas raras. 2) Ajuste Proporcional/Derivativo/Integral Ajustar K hasta que el sistema sobre pase o empiece a sobre oscilar Aumentar Kd hasta que empiezan a pasar cosas raras Ajustar Ki hasta eliminar error estacionario Con los dos se van a conseguir resultados similares, en último término. El autor aconseja el primero. El motivo es que el término derivativo es muy sensible al ruido de la medición y, por tanto, es difícil de ajustar en un caso real. V. IMPLEMENTAR UN CONTROLADOR CON MICROCONTROLADORES Ahora toca dejar la teoría, desempolvar el teclado y ver cómo se implementa un controlador PID en un microcontrolador. Afortunadamente, es bastante sencillo hacer un PID básico. A. PID a Mano Primero se va a ver cómo implementar un controlador sencillo. En realidad, el código no es excesivamente complejo. Supongamos que el controlador toma la entrada de la variable controlada en A0, y efectúa la salida mediante una señal PWM en el pin 3. El código de un PID básico podría ser el siguiente: // Asignaciones pins const int PIN_INPUT = A0; const int PIN_OUTPUT = 3; // Constantes del controlador double Kp=2, Ki=5, Kd=1; // variables externas del controlador: double Input, Output, Setpoint; // variables internas del controlador: unsigned long currentTime, previousTime; double elapsedTime; double error, lastError, cumError, rateError; void setup() { Input = analogRead(PIN_INPUT); Setpoint = 100; } void loop() { pidController.Compute(); // leer una entrada del controlador Input = analogRead(PIN_INPUT);
Como implementar un PID con Microcontroladores 7 // calcular el controlador Output = computePID(Input); delay(100); // escribir la salida del controlador analogWrite(PIN_OUTPUT, Output); } double computePID(double inp) { // obtener el tiempo actual currentTime = millis(); // calcular el tiempo transcurrido elapsedTime = (double)(currentTime - previousTime); // determinar el error entre la consigna y la medición error = Setpoint - Input; // calcular la integral del error cumError += error * elapsedTime; // calcular la derivada del error rateError = (error - lastError) / elapsedTime; // calcular la salida del PID double output = kp*error + ki*cumError + kd*rateError; // almacenar error anterior lastError = error; // almacenar el tiempo anterior previousTime = currentTime; return output; } Como se puede observar, no es demasiado difícil. Hay una función computePID() que realiza todo el trabajo. En esta función se calcula el tiempo transcurrido entre llamadas, que es necesario para calcular tanto la derivada como la integral del error. A continuación, se compara la entrada del controlador con la consigna para determinar el error y se realizan las 'pseduo' integrales y derivadas (su equivalente en discreto). Finalmente, se calcula la respuesta del sistema mediante la fórmula del PID, y se guardan los valores para el siguiente ciclo. Sin embargo, aunque es completamente funcional, este controlador PID es bastante simple. Por tanto, tiene ciertas carencias ante situaciones que ocurren frecuentemente en la realidad. B. Reset wind-up Reset windup es una trampa en la que caen muchos principiantes. Ocurre cuando el PID cree que está haciendo algo que no puede. Por ejemplo, un PWM que acepta valores entre 0 y 255. El PID por defecto no sabe esto. Si cree que los valores 300, 400, 500 funcionarán, probará esos valores esperando conseguir lo que necesita. Dado que en realidad el valor está saturado a 255 seguirá intentando valores cada vez más alto sin llegar a ninguna parte. El problema se presenta en forma de retrasos raros. C. La Solución 1) Primer paso Existen diversas maneras para mitigar el Wind-up, pero la forma más intuitiva es indicarle al PID cuáles son los límites de salida. En el código que se ve más adelante hay una function SetOutputLimits. Cuando uno de los limites es alcanzado, el PID deja de sumar (Integrar). 2) Segundo paso Hay aún diferencia entre lo que el PID cree que está enviando y lo que en realidad envía. Esto es debido al componente Proporcional y (en menor medida) al termino derivativo. Aunque el componente integral ha sido saturado de forma segura, P y D están aún contribuyendo al wind-up. Esto puede ser inaceptable en según que aplicación. Así que como segundo paso se satura el valor de salida también. /*working variables*/ unsigned long lastTime; double Input, Output, Setpoint; double ITerm, lastInput; double kp, ki, kd; int SampleTime = 1000; //1 sec double outMin, outMax; void Compute() { unsigned long now = millis(); int timeChange = (now - lastTime); if(timeChange>=SampleTime) { /*Compute all the working error variables*/ double error = Setpoint - Input; ITerm+= (ki * error); if(ITerm> outMax) ITerm= outMax; else if(ITerm< outMin) ITerm= outMin; double dInput = (Input - lastInput); /*Compute PID Output*/ Output = kp * error + ITermkd * dInput; if(Output > outMax) Output = outMax; else if(Output < outMin) Output = outMin; /*Remember some variables for next time*/ lastInput = Input; lastTime = now; }