Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale

Equation Chapter 1 Section 1 Proyecto Fin de Carrera Grado en Ingeniería de las Tecnologías Industriales Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Autor: Alejandro Romero Galán Tutor: Daniel Limón Marruedo Dep. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2015 iii Proyecto Fin de Carrera Ingeniería de Telecomunicación Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Autor: Alejandro Romero Galán Tutor: Daniel Limón Marruedo Profesor titular Dep. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2015 v Proyecto Fin de Carrera: Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Autor: Alejandro Romero Galán Tutor: Daniel Limón Marruedo El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros: Presidente: Vocales: Secretario: Acuerdan otorgarle la calificación de: Sevilla, 2015 El Secretario del Tribunal vii A todo aquel que creyó en mí ix Agradecimientos riesgo de hastiar al lector del presente trabajo, no debo olvidar a ninguno de aquellos que hicieron posible que este proyecto saliese adelante. En primer lugar, dar las gracias a mi padre y a mi madre, que siempre han estado dispuestos a asentir con la cabeza aún cuando lo que merecía era un ‘no’ por respuesta. A ellos les dedico y les debo todo lo que he hecho de provecho en mi vida, y lo que haga en la siguiente. Agradezco profundamente a mis hermanos por aguantar preguntas sin sentido (el mayor), una ardua convivencia (el pequeño) y un carácter difícil que no merece perdón ni atención (ambos). Quiero mencionar individualmente a cada uno de los compañeros que, junto a mi persona, componen la asociación universitaria EsiTech y han trabajado activamente en el proyecto AirWhale: José Luis Holgado Álvarez, Elena Manga Caballero, Juan Carlos Mancebo Sánchez, Javier Eduardo Mitjavila Samayoa, Inmaculada Gómez Vázquez, Miguel Ángel Rodrigo Lisbona, Juan Carlos Martín Rodríguez y Ana Caballos Torroba. Ha sido un honor trabajar y reír a vuestro lado; no podría haber completado este escrito sin vuestra ayuda. A mi tutor, Dani, le debo todo lo que he hecho en casi un año de ideas, intenciones, caminos sin salida y hechos; y la pasión por lo que hacía, que he ido adquiriendo conforme trabajaba en este proyecto. La dedicación que ha empleado en temas, a priori, desconocidos para él, ha sido un gran referente en el camino del autoaprendizaje. De forma general, porque si tuviese que mencionar a cada uno de ellos este trabajo sería interminable, gracias a todos aquellos compañeros de universidad que en algún momento me hablaron, me consolaron, me animaron, me hicieron reir, me ilusionaron o, simplemente, me consideraron una persona digna de entablar amistad con ellos. Nunca os olvidaré. Debo nombrar a mis compañeros de piso en Sevilla, a Carlos, Javi y Manolete, por suponer aquella válvula de escape en los momentos en los que necesitaba desconectar de tantos números, códigos y dificultades. Gracias, Ester, por aguantarme. Por ser el motor que ha movido todo esto. Por último, no pienso olvidarme de agradecer a todos aquellos españoles que, pasando verdaderos problemas para llegar a fin de mes y poder darles a sus hijos una vida digna y plena de derechos, han seguido colaborando económicamente con mi formación académica (y mi salud), incluso cuando esa élite corrupta e indigna, que se hacen llamar políticos, no ha hecho más que colocarles más peso en su malograda espalda. Espero, algún día, devolverles cada uno de los euros que han invertido en mí con mis futuros trabajos. A 4.2 Sensores 25 4.3 Almacenamiento 28 4.4 Alimentación 29 4.5 Actuadores 30 4.5.1 Motores principales 30 4.5.2 Hélices propulsoras 31 4.5.3 Servomotores 32 4.6 Variadores/reguladores 33 4.7 Conexionado y otros componentes 34 4.8 Balance de pesos 37 4.9 Presupuesto final 37 5 Firmware ArduCopter y creación de un firmware para la aeronave AirWhale 39 5.1 El firmware ArduCopter 39 5.1.1 Librerías 40 5.1.2 Arducopter.pde 54 5.2 Códigos desarrollados 56 5.2.1 Código test_sensores_actuadores.pde 58 5.2.2 Código sintonizacion.pde 71 5.2.3 Función captura_datos 74 6 Construcción del prototipo AirWhale y experimentación 76 6.1 Introducción: geometría y movimientos del prototipo 76 6.2 Materiales de construcción y dispositivos electrónicos a usar 79 6.3 Proceso de construcción 79 6.4 Conexionado 87 6.5 Experimentos realizados 89 6.5.1 Estabilización estática 89 6.5.2 Identificación del prototipo: sistema real 91 6.5.3 Estabilización dinámica de los ángulos roll y pitch 93 7 Conclusiones y posibles trabajos futuros 99 ANEXO A 101 ANEXO B 105 ANEXO C 112 Referencias 134 xvii ÍNDICE DE TABLAS Tabla 4.1. Pesos de los distintos elementos de la aeronave AirWhale y balance 37 Tabla 4.2. Presupuesto final 38 xix ÍNDICE DE FIGURAS Figura 1.1. Diseño conceptual actualizado de la aeronave AirWhale [1] 1 Figura 1.2. Diseño inicial del AirWhale [1] 3 Figura 1.3. Esquema de la organización de la asociación EsiTech (a extinguir) 5 Figura 1.4. Logo de la asociación universitaria EsiTech 5 Figura 1.5. Cartel promocional del AirWhale 6 Figura 1.6. Cartel de presentación del AirWhale en 6 Figura 2.1. Esquema de trabajo del equipo Automático 8 Figura 2.2. Plataforma PIXHAWK 9 Figura 2.3. Plataforma OpenPilot CC3D 10 Figura 2.4. Plataforma OpenPilot Revolution 10 Figura 2.5. Plataforma AUAV3 11 Figura 2.6. Evolución visual de la plataforma ArduPilot, desde su primera versión hasta la 2.6 [13]. 12 Figura 2.7. Esquema de las conexiones de la placa APM 2.6 [13] 13 Figura 2.8. Interfaz principal del software Mission Planner. 14 Figura 3.1. Ilustración de los sistemas de coordenadas empleados [3] 17 Figura 3.2. Esquema de la relación entre subsistemas de traslación y rotación 21 Figura 4.1. Módulo 3DR uBlox GPS con magnetómetro incorporado 26 Figura 4.2. 3DR Power Module 26 Figura 4.3. 3DR Radio Set 27 Figura 4.4. Sónar XL-MAXSONAR-EZ MB1240 28 Figura 4.5. Dispositivo de almacenamiento DataTraveler microDuo 3.0 28 Figura 4.6. Batería Zippy 5000mAh 3S 20C-30C 29 Figura 4.7. Motor Brushless EMAX BL2210/30 31 Figura 4.8. Hélice 9x4.7 de fibra de carbono. 32 Figura 4.9. Servomotor TowerPro SG92r 9G 33 Figura 4.10. Regulador Brushless EMAX 18A Budget 33 Figura 4.11. EMAX Program Card. 34 Figura 4.12. Placa de distribución de corriente 35 Figura 4.13. Adaptador XT60 macho t-dean hembra. 35 Figura 4.14. Conector t-dean macho-hembra. 36 Figura 4.15. Conector XT60 macho-hembra. 36 Figura 4.16. Cargador IMAX B6AC. 36 Figura 5.1. Esquema principal del filtrado DCM [28] 50 Figura 5.2. Diagrama de flujo principal del código Arducopter.pde 55 Figura 5.3. Cronograma del trabajo desarrollado en este escrito, detallando los archivos de código creados 57 Figura 5.4. Diagrama de flujo del funcionamiento general de test_sensores_actuadores.pde 59 Figura 5.5. Diagrama de flujo de la subrutina test_motores 60 Figura 5.6. Diagrama de flujo de la subrutina test_acel_gir 61 Figura 5.7. Diagrama de flujo de la subrutina test_barom 62 Figura 5.8. Diagrama de flujo de la subrutina test_AHRS. 63 Figura 5.9. Resultados experimento 1: presión sin calibrar 64 Figura 5.10. Resultados experimento 1: temperatura sin calibrar 64 Figura 5.11. Experimento 1: altitud calibrada. 65 Figura 5.12. Experimento 1: presión calibrada 65 Figura 5.13. Experimento 1: temperatura calibrada 65 Figura 5.14. Experimento 2: altitud calibrada 66 Figura 5.15. Experimento 2: presión calibrada 66 Figura 5.16. Experimento 2: temperatura calibrada 67 Figura 5.17. Secuencia de movimientos para los experimentos con el acelerómetro/giróscopo 67 Figura 5.18. Experimento 1: aceleraciones lineales sin calibrar 68 Figura 5.19. Experimento 2: velocidades angulares sin calibrar 68 Figura 5.20. Experimento 1: aceleraciones lineales calibradas. 69 Figura 5.21. Experimento 1: velocidades angulares calibradas. 69 Figura 5.22. Experimento 2: aceleraciones lineales calibradas. 70 Figura 5.23. Experimento 2: velocidades angulares calibradas. 70 Figura 5.24. Resultados del experimento con el estimador DCM. 71 Figura 5.25. Diagrama de flujo del funcionamiento general de sintonizacion.pde 73 xxi Figura 6.1. Diseño conceptual del prototipo en CATIA [4] 76 Figura 6.2. Esquema básico e inicial del prototipo 77 Figura 6.3. Esquema de la causa del par en X no deseable al girar los motores traseros el mismo ángulo 77 Figura 6.4. Esquema básico y final del prototipo 78 Figura 6.5. Construcción del ala 80 Figura 6.6. Medidas de los dos lados superiores con las alas colocadas 80 Figura 6.7. Estructura principal 81 Figura 6.8. Detalle de la unión entre lado menor y mayor de la estructura principal 81 Figura 6.9. Soportes verticales colocados en la estructura principal 82 Figura 6.10. Trozo de madera con muesca indicada, usada para la unión de los soportes 82 Figura 6.11. Detalle de la unión de los soportes con la estructura principal 82 Figura 6.12. Detalle de la unión de la plataforma inferior con los soportes (cara interior) 83 Figura 6.13. Detalle de la unión de la plataforma inferior con los soportes (cara exterior) 83 Figura 6.14. Fisura encontrada al retomar la construcción del prototipo 84 Figura 6.15. Ala fijada de nuevo y reforzada 84 Figura 6.16. Listón colocado para formar un perfil en T junto con los listones del lado mayor 85 Figura 6.17. Corte realizado en el ala para eliminar la pérdida de sustentación aportada por el motor 85 Figura 6.18. Plataforma de madera usada para albergar los motores verticales contribuyentes al par en Y. Nótese la marca de la posición anterior de dichos motores. 86 Figura 6.19. Prototipo construido 86 Figura 6.20. Conexionado de los variadores y motores en paralelo con la batería 87 Figura 6.21. Conexionado placa-variadores 88 Figura 6.22. Conexión T-DEAN para la alimentación del variador alimentador por BEC. 88 Figura 6.23. Adaptador XT60 – T-DEAN conectado al cableado de alimentación 88 Figura 6.24. Experimentos realizados para la estabilización estática del vehículo sobre el eje Y 89 Figura 6.25. Estabilización sobre el eje Y conseguida 90 Figura 6.26. Experimento realizado para la estabilización estática sobre el eje X 90 Figura 6.27. Estabilización estática sobre eje X conseguida 91 Figura 6.28. Peso colocado debajo del ala para la estabilización 91 Figura 6.29. Representación del valor del ángulo roll ante la aplicación en bucle abierto de un par en X 92 Figura 6.30. Representación del valor del ángulo pitch ante la aplicación en bucle abierto de un par en Y 92 Figura 6.31. Evolución del ángulo roll y pitch durante la estabilización del ángulo roll y perturbaciones 93 Figura 6.32. Evolución del error de los ángulos con respecto a la referencia durante la estabilización del ángulo roll 93 Figura 6.33. Par de actuación durante la estabilización del ángulo roll y perturbaciones 94 Figura 6.34. PWM mandados a los motores laterales durante la estabilización del ángulo roll 94 Figura 6.35. Evolución de los ángulos roll y pitch en la estabilización del ángulo pitch y perturbaciones 95 Figura 6.36. Evolución del error de los ángulos con respecto a la referencia durante la estabilización del ángulo pitch 95 Figura 6.37. Par de actuación durante la estabilización del ángulo pitch y perturbaciones 96 Figura 6.38. PWM mandados a los motores laterales durante la estabilización del ángulo pitch 96 Figura 6.39. Evolución de los ángulos roll (azul) y pitch (verde), y de la altitud (rojo) 97 Figura 6.40. Evolución del error con respecto a la referencia de los ángulos roll y pitch 97 Figura 6.41. Evolución de los pares de actuación asociados al control del roll (azul) y del pitch (verde) 98 Figura 6.42. Evolución de las actuaciones individuales de cada uno de los cuatro motores: dos motores contribuyentes al par en X (verde y cián) y dos motores contribuyentes al par en Y (azul y rojo) 98 xxiii Notación A* Conjugado c.t.p. En casi todos los puntos c.q.d. Como queríamos demostrar ∎ Como queríamos demostrar e.o.c. En cualquier otro caso E número e IRe Parte real IIm Parte imaginaria sen Función seno Tg Función tangente arctg Función arco tangente sen Función seno sinxy Función seno de x elevado a y cosxy Función coseno de x elevado a y Sa Función sampling Sgn Función signo Rect Función rectángulo Sinc Función sinc ∂y ∂x x◦ Derivada parcial de y respecto Notación de grado, x grados. Pr(A) Probabilidad del suceso A SNR Signal-to-noise ratio MSE Minimum square error : Tal que < Menor o igual > Mayor o igual \ Backslash ⇔ Si y sólo si 7 1 INTRODUCCIÓN l concepto de mejorar una carencia de una tecnología considerablemente en auge no es nuevo. Es algo inherente al ingeniero o al científico: intentar ir más allá, partiendo desde donde lo dejaron sus semejantes. Aportar algo al conocimiento tecnológico actual desde un punto de vista práctico. En este sentido, nació el proyecto AirWhale. 1.1 El Proyecto AirWhale Hace aproximadamente un año, al final del curso académico 2013/2014, un grupo de alumnos, entre los que me incluyo, de distintos grados de ingeniería, impartidos en la Escuela Superior Técnica de Ingenieros de la Universidad de Sevilla, se embarcó en la difícil tarea de idear una tecnología que supliese la falta de una gran autonomía que caracteriza al actualmente famoso multirotor. Bien es conocido este problema inherente a estas aeronaves, provocado, principalmente, por el peso considerable de los componentes electrónicos necesarios para su funcionamiento (motores, variadores, baterías, sensores, autopiloto…) y la estructura en sí, y por la necesaria actuación ininterrumpida de los rotores para poder mantener al multirotor en el aire, de manera que, aún reduciendo el peso, la duración de las baterías decrece de manera drástica. Ante este problema, se adaptó la idea básica concebida por el alumno José Luis Holgado Álvarez de usar la sustentación del helio para eliminar parte del peso que los rotores debían vencer y reducir la actuación de los mismos de forma continuada. Así, surgió el AirWhale. Un híbrido entre multirotor y dirigible. La idea es sencilla: se mantiene la agilidad, la velocidad y la capacidad de respuesta que aportan los distintos rotores con los que contaba el multirotor, y se añade autonomía y estabilidad acoplando a la estructura un volumen de helio confinado para sustentar gran parte del peso de la aeronave. Figura 1.1. Diseño conceptual actualizado de la aeronave AirWhale [1] E “Si he logrado ver más lejos, ha sido porque he subido a hombros de gigantes” Isaac Newton Equipo Automático: organización e introducción a mi trabajo personal 8 8 mediante el vuelo del mismo, observando los resultados y su desviación con los obtenidos de forma teórica. Esta tarea corresponde al alumno José Luis Holgado y a mí, y puede encontrarse redactado en el Capítulo 6. Ante esta información, es clara la relación existente entre el trabajo de cada uno de los miembros del equipo Automático, la cual se muestra en el esquema de la Figura 2.1. Figura 2.1. Esquema de trabajo del equipo Automático Las relaciones clave se dan una vez se hayan diseñado las leyes de control y el modelo dinámico, ya que ello supone completar los recursos necesarios para la completa modificación del firmware ya existente y el diseño del sistema hardware-in-the-loop en su fase correspondiente al software Matlab/Simulink. Por otro lado, el firmware modificado es necesario para la verificación teórica obtenido del sistema HIL, y viceversa, y para poder volar el prototipo correctamente. Aún existiendo estas relaciones, cada uno de los integrantes del equipo ha podido trabajar individualmente dentro de unos límites amplios. 2.2 Introducción a la plataforma ArduPilot 2.2.1 Estado del arte La necesidad de una plataforma autopiloto es clara. Independientemente de la aeronave que se esté tratando, el uso de un autopiloto es totalmente imprescindible para la implementación de las leyes de control y ecuaciones necesarias para el movimiento autónomo de la misma. Actualmente, existen varias plataformas a disposición del interesado para su uso en el control de aeronaves, cada una con características propias que las hace interesantes en uno u otro sentido. A continuación se recogen algunas de las más populares, por sus grandes prestaciones y relevancia por su pertenencia a proyectos más ambiciosos que incluyen desarrollos de firmwares y otros componentes electrónicos, paralelos al Proyecto ArduPilot. Selección electrónica (sensores, actuadores, autopiloto...) y compra Construcción de prototipo Verificación y experimentación Revisión firmware ArduCopter Modificación ArduCopter: leyes de control y modelo dinámico Verificación teórica Diseño de modelo simulable y validación Diseño de leyes de control, implementación y validación Diseño sistema HIL e interfaz Matlab 9 9 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale 2.2.1.1 PX4 PIXHAWK La plataforma PX4 PIXHAWK puede resultar una de las alternativas más interesantes al ArduPilot. Pertenece al Proyecto PX4, que también goza de una comunidad de desarrolladores que han llevado a cabo un firmware independiente, de un programa que permite la visualización de datos y la calibración de los sensores como es QGroundControl (análogo a Mission Planner) y, además, permite la implementación de firmwares desarrollados por el Proyecto ArduPilot, aumentando considerablemente la versatilidad de su placa insignia [6]. Las características de la plataforma se muestran a continuación: Figura 2.2. Plataforma PIXHAWK  Procesador de 32 bits STM32F427 Cortex M4 Core con FPU (unidad de punto flotante), de 168 MHz, 256 kB de memoria RAM, 2 Mb de memoria FLASH. También dispone de un coprocesador de 32 bits STM32F103.  Cuenta con giróscopo ST Micro L3GD20H de 16 bits, acelerómetro/magnetómetro ST Micro LSM303D de 14 bits, acelerómetros de 3 ejes/giróscopo Invensense MPU 6000, y un barómetro MEAS MS5611.  Dispone de 5 puertos serie (uno de alta capacidad, dos de control de flujo de datos), dos puertos de protocolo CAN (Controller Area Network), entrada compatible con los DSM de Spektrum (radiocontrol), entrada y salida dedicada al Futaba S.BUS (conexión de múltiples servos), entrada de señal PPM, entrada para PWM o voltaje, I2C, SPI, entrada para GPS, convertidores analógico-digital de 3.3 V y 6.6 V y puerto interno microUSB.  14 salidas PWM, seis auxiliares y de alta potencia [6]. 2.2.1.2 OpenPilot CopterControl y OpenPilot Revolution Plataformas pertenecientes al Proyecto OpenPilot, nacido en 2010 de la mano de una comunidad de usuarios de todo el mundo. Dicho proyecto, aunque lleva pocos años en activo, es bastante completo, y supone una interesante opción a la hora de adquirir un sistema autopiloto. Además de desarrollar su propio firmware y software de adquisición de datos, el Proyecto OpenPilot sostiene como estandartes dos plataformas, por encima de otros componentes electrónicos propios, como sensores GPS y telemetría:  OpenPilot CopterControl: Consiste en una plataforma autopiloto para la estabilización de aeronave multirotor, helicópteros y aviones no tripulados. Su versión actualizada, la plataforma OpenPilot CC3D, consta de las siguientes características: Equipo Automático: organización e introducción a mi trabajo personal 10 10 Figura 2.3. Plataforma OpenPilot CC3D o Procesador ARM STM32F1 de 32 bits Cortex M3, con 512 Kb de FLASH y 64 Kb de RAM, de 72 MHz [7]. o Giróscopo/Acelerómetro MPU 6000. No posee barómetro. o 6 canales PWM; incluye PPM, soporte para Spektrum DSM2, DSMJ, DSMX; soporte para Futaba S.BUS, entrada para telemetría y GPS (diseñado por el propio Proyecto OpenPilot), I2C (en desarrollo), 10 salidas PWM con compatibilidad con cámara y entrada USB para fácil configuración [8].  OpenPilot Revolution: El Proyecto OpenPilot decide mejorar considerablemente las capacidades de la plataforma CopterControl mediante la creación de la OpenPilot Revolution, que incluye una mayor cantidad de conexiones útiles, un procesador de mayor rapidez y memoria y la implementación de sensores a día de hoy muy utilizados como son el barómetro o el sónar (añade la posibilidad de conectarlo). Las características generales son: Figura 2.4. Plataforma OpenPilot Revolution 11 11 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale o Procesador ARM de 32 bits STM32F4 Cortex M4, de 168 MHz, con 192 Kb de memoria RAM y 1 Mb de FLASH, con FPU. o Sensor acelerómetro/giróscopo de 3 ejes MPU 6000, barómetro MS5611-01BA03 con sensibilidad mínima de 10 cm y magnetómetro Honeywell HMC5883L con sensibilidad de 1- 2º. o 6 canales PWM de recepción junto con dos GPIO, un canal PPM de recepción junto con cinco puertos entrada/salida PWM para conexión de motores, servos y/o sensores; cuatro pines SPI, un pin UART, protocolo CAN, soporte para I2C, telemetría, GPS, Spektrum DSM, Futaba S.BUS y sónar; puerto SWD para facilidad de depuración para desarrolladores, seis salidas específicas PWM y microUSB [9]. 2.2.1.3 Arsov AUAV3 Esta plataforma, menos conocida que las anteriores, está inspirada en el Proyecto PX4 y en su plataforma. El objetivo de este autopiloto es, sin embargo, suponer una alternativa seria y económica a su análogo aportado por el Proyecto PX4, igualando en gran medida sus prestaciones y reduciendo su precio. También es remarcable la compatibilidad con los firmwares desarrollados para el ArduPilot, aunque ya se está trabajando en un firmware exclusivo para la plataforma AUAV3. Algunas de sus especificaciones son: Figura 2.5. Plataforma AUAV3  Microprocesador dsPIC33EP512MU810 Microchip de 16 bits de 70 MHz, 512 Kb de RAM y 32 Mb de FLASH AT45DB321 incorporado [10] [11].  Sensores IMU MPU6000, magnetómetro HMC5883 y barómetro BMP180.  3 pines UART, dos de ellos destinados a la telemetría y al OSD; 8 canales de entrada y salida, 4 entradas analógicas, 3 pines I/O digitales, soporte para GPS, I2C, SPI y protocolo CAN (driver MAX3051), entre otros [11]. 2.2.1.4 Proyecto Paparazzi El Proyecto Paparazzi es uno de los proyectos públicos relacionados con los vehículos aéreos no tripulados más completos que se pueden encontrar. Abarca todo el sistema autopiloto, desde la plataforma hasta el firmware, pasando por los distintos periféricos que se pueden usar en conjunto con ambos. Desde su fundación en 2003, miles de desarrolladores se han sentido identificados con las pretensiones del proyecto, muchos de ellos vinculados a universidades de Francia, Holanda o Estados Unidos. También, a raíz Equipo Automático: organización e introducción a mi trabajo personal 12 12 del proyecto, han surgido, de la mano de desarrolladores, empresas que comercializan productos relacionados con el proyecto o se especializan en el desarrollo de los mismos. No se puede decir, sin embargo, que exista una plataforma específica y característica del proyecto, como se ha visto en los anteriores. El Proyecto Paparazzi ha llevado a cabo una labor de experimentación en multitud de tarjetas para validar el firmware propio desarrollado. La lista, relativamente extensa, se puede consultar en la página principal del proyecto [12]. 2.2.2 Proyecto ArduPilot La plataforma ArduPilot es resultado de años de trabajo en el denominado Proyecto ArduPilot. El nombre proviene de ‘Arduino’, ya que las primeras versiones de la plataforma (APM 1) se basaban en el entorno de desarrollo Arduino. Conforme se fue actualizando dicha plataforma, el firmware asociado a la misma evolucionaba paralelamente, y era compatible con más tipos de hardware, y no sólo con aquellos compatibles con Arduino, como el PIXHAWK. Por ello, hoy día la plataforma recibe el nombre de APM (ArduPilot Mega), al igual que los firmwares asociados (APMCopter, APMPlane…), siendo sus versiones más recientes APM 2.5 y APM 2.6 [13]. El proyecto abarca el desarrollo de plataformas autopiloto, la programación de firmwares para el control de distintos vehículos de uso popular como son el avión, el multirotor o los rovers; y la creación de un software que haga de conexión entre el usuario y el hardware, llamado Mission Planner. En las siguientes subsecciones se repasará de forma general cada uno de los componentes principales del Proyecto Ardupilot, posponiendo el estudio en profundidad al siguiente capítulo. 2.2.2.1 Plataforma ArduPilot El cerebro del AirWhale. Esta placa será la que lleve a cabo las operaciones necesarias para que los rotores actúen adecuadamente y la aeronave pueda volar según las leyes de control diseñadas. La versión usada en el proyecto es el ArduPilot Mega 2.6 o APM 2.6, pero la plataforma ha experimentado muchos cambios para llegar a disponer de las características de las que goza a día de hoy. En la Figura 2.6 se repasa una evolución de la plataforma desde 2009 hasta 2012. Figura 2.6. Evolución visual de la plataforma ArduPilot, desde su primera versión hasta la 2.6 [13]. 13 13 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Las especificaciones técnicas de las que dispone la versión APM 2.6 son las siguientes:  Procesador Atmel ATMEGA2560 de 8 bits, con memoria FLASH de 256 Kb, 8 Kb de RAM y 16 MHz [14]. También cuenta con un coprocesador Atmel ATMEGA32U2 de 8 bits, con memoria FLASH de 32 bits, 1 Kb de RAM y 16 MHz [15] y un chip DataFlash de 4 Mb para el registro de datos automático.  Cuenta con tres sensores a bordo: un acelerómetro/giróscopo MPU6000 y un barómetro MS5611- 01BA03 de Measurements Specialties. No cuenta con un magnetómetro a bordo, como su predecesor APM 2.5, sino que está adaptado para que el conecte externamente, evitando así las interferencias magnéticas que pueda producir la propia placa [13].  8 salidas y entradas digitales, cada una de ellas con pines polarizados y el neutro. Pensando en la práctica, estos pines se emplean con dos cometidos: o Las salidas se usan para alimentar a los motores, el sensor de flujo óptico (no usado en este proyecto), para actuar como puerto RSSI y para manejar la cámara (servos). En el momento de detallar el firmware ArduCopter, se entrará en mayor profundidad en la función de estos pines. o Las entradas son, en su mayoría, utilizadas para los canales de radiocontrol. Normalmente unos pocos suelen quedar sin función, en ese caso se usan como puertos auxiliares para otros cometidos (se verá detallado en las siguientes secciones) [13].  Entradas adaptadas para el GPS, la alimentación mediante un módulo de alimentación, I2C (normalmente usado para la conexión de un magnetómetro externo) y telemetría [13].  9 entradas analógicas (A0-A8) para utilidades generales (sónar, sensor de velocidad del aire…) y otras cinco, dos de ellas como puertos SPI (A12-A13) y las otras tres como entrada/salida (A9-A11) [13]. La figura 2.7 muestra un esquema general de las conexiones descritas. Figura 2.7. Esquema de las conexiones de la placa APM 2.6 [13] En el Capítulo 4 se enumerarán y detallarán los dispositivos que se conectarán a la plataforma. 2.2.2.2 Software Mission Planner La interfaz placa-usuario. Este programa recoge un sinfín de opciones e información disponible sobre los disintos aspectos del hardware y firmware del ArduPilot. El mismo se puede descargar gratuitamente desde el Equipo Automático: organización e introducción a mi trabajo personal 14 14 enlace [16]. En la Figura 2.8 puede apreciarse la interfaz principal del programa Mission Planner. Figura 2.8. Interfaz principal del software Mission Planner. Se explicarán, en orden de numeración, los distintos módulos que aparecen en la Figura 2.8: 1. Menú principal: Aquí aparecen las principales secciones que componen el programa. Se describirán generalmente las más importantes:  Flight Data (seleccionada)  Flight Plan: Permite planificar el recorrido de la aeronave seleccionando puntos clave del camino a seguir o ‘waypoints’, y otras opciones relacionadas con las misiones.  Initial Setup: Proporciona indicaciones para configurar la plataforma ArduPilot, incluyendo la actualización del firmware o la correcta calibración de los sensores.  Config/Tuning: Permite configurar varias opciones relacionadas con el software y firmware (PIDs, parámetros que permiten habilitar o deshabilitar hardware…).  Simulation: Sección donde se puede observar una simulación basada en otros softwares de simulación (FlightGear, X-Plane, JSBSim…) si está activado el hardware-in-the-loop. 2. Visualización de piloto: Visión ‘a bordo’ del movimiento de la aeronave. Se detallan los giros en las tres direcciones, la altitud, la intensidad de señal en porcentaje, el tiempo de vuelo, la velocidad del aire (en caso de no disponer de sensor de velocidad del aire, marca la velocidad del suelo), el estado de la batería y del GPS, distancia al siguiente ‘waypoint’ y más información. 3. Registro de datos: Muestra distintos datos relacionados con el vuelo: medidas de sensores (acelerómetro, giróscopo, sensor de presión…), velocidades, posición o actuaciones. También pueden revisarse mensajes (recibidos por protocolo Mavlink), descargar registros de datos almacenados y emplearse otras interfaces visuales. 4. Mapa: Muestra la situación actual de la aeronave en un mapa proporcionado por Google Maps. Esto es posible gracias a los datos proporcionados por el GPS. 5. Conexión APM: Aquí se selecciona el puerto serie donde se encuentra la plataforma ArduPilot, la velocidad a la que se obtienen los datos por este puerto y se activa la conexión. Al activar la conexión, nuevas opciones pueden aparecer en el software. 1 2 3 4 5 15 15 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Aunque la utilidad del software es relevante, para este proyecto no se usará demasiado, limitando su utilidad a la comprobación del buen funcionamiento de los sensores (aunque fue realizado por el alumno, no se detallará en este trabajo; se demostrará el funcionamiento óptimo de los sensores a utilizar a nivel de código, en el Capítulo 5). 2.2.2.3 Firmware ArduPilot La inteligencia de la aeronave. Consiste en un código en lenguaje C/C++ que recoge las sentencias precisas para el funcionamiento correcto de la placa APM 2.6, para una comunicación adecuada con el software Mission Planner y el uso deseado de los dispositivos electrónicos conectados a la plataforma. Este firmware es muy extenso, abarcando cientos de archivos de código divididos en grandes grupos pertenecientes a cada vehículo soportado por la comunidad de desarrolladores. Estos grupos son:  APMrover: Tiene como objetivo la creación de un firmware estable y eficaz para vehículos terrestres no tripulados.  ArduPlane: Busca el desarrollo de un firmware adecuado para aeronaves con configuración parecida a la de un avión clásico.  ArduCopter: El firmware que atañe a este escrito. Aporta un repositorio de archivos muy extenso para hacer volar un multirotor de forma óptima. Además, existen, de forma común, varios ficheros relacionados con librerías, utilidades varias para los desarrolladores y módulos relacionados con otras plataformas (PIXHAWK). Concretamente, el firmware ArduCopter consta de 69 archivos, entre los que se hallan archivos de código C++, librerías concretas para el ArduCopter y otros. Por otro lado, las librerías comunes, recogen conceptos relacionados con sensores, comunicación con Mission Planner, declaraciones a nivel de placa, controladores como el PID, funciones matemáticas o sentencias necesarias para la compatibilidad con otras plataformas autopiloto. Más adelante se detallarán en mayor profundidad los archivos relativos al presente trabajo. Modelo dinámico del AirWhale 16 16 3 MODELO DINÁMICO DEL AIRWHALE onocer cómo debe comportarse la aeronave AirWhale es uno de los hitos a alcanzar por el equipo Automático. Por ello, uno de los trabajos que se expondrán aquí es la obtención de unas ecuaciones que definan completamente la dinámica de una primera versión del AirWhale (prototipo a construir). Este trabajo, como ya se ha mencionado anteriormente, ha sido desarrollado conjuntamente con el alumno José Luis Holgado Álvarez (se podrá encontrar el mismo trabajo en su escrito [4]) y se ha basado en el modelo dinámico desarrollado por nuestro compañero del equipo Mecánico, Javier Eduardo Mitjavila Samayoa. A continuación se expondrá dicho modelo a modo de introducción y, posteriormente, se desarrollarán las transformaciones necesarias para obtener el modelo susceptible a traducción en Simulink. 3.1 Modelo dinámico del AirWhale (formulación cartesiana y transformación a coordenadas de Euler) El modelo dinámico del AirWhale en coordenadas cartesianas ha sido desarrollado íntegramente por el alumno Javier Mitjavila en su Trabajo Fin de Grado [3]. Este modelo tiene en cuenta todas las fuerzas que actúan sobre la aeronave, pero aquí se hará una simplificación para reducir la complejidad del modelo que se simulará posteriormente sin reducir significativamente la precisión, teniendo en cuenta que lo que se pretende construir es un prototipo de dinámica parecida al AirWhale. En ese sentido, no se han tenido en cuenta la sustentación de las alas (debido al pobre diseño de las mismas en el prototipo y que se supone que el ángulo de cabeceo se controlará para que no sufra variaciones significativas) ni las fuerzas ni momentos de masa añadida (ver [3] para más información). El modelo parte de la siguiente ecuación general: 𝑀𝑚𝑎𝑠𝑎∙𝑞󰇘=𝜏𝐼+𝜏𝐺+𝜏𝐶+𝜏𝐴𝑆+𝜏𝐴𝐷=𝜏𝑇𝑂𝑇𝐴𝐿 Donde:  𝑀𝑚𝑎𝑠𝑎 es una matriz 6x6 que modela la masa del AirWhale.  El vector de 6 componentes 𝜏𝐼 representa las fuerzas de inercia que afectan a la aeronave.  𝜏𝐺 corresponde a las fuerzas de gravedad.  𝜏𝐶 recoge las fuerzas de control.  𝜏𝐴𝑆 representa las fuerzas de sustentación del helio.  𝜏𝐴𝐷 corresponde a las fuerzas aerodinámicas que sufre el AirWhale.  𝑞󰇘 representa la segunda derivada temporal del vector de estados. La ecuación (1) y siguientes se basan en los sistemas de coordenadas que aparecen en la Figura 3.1. En ella se puede observar cómo el eje X del sistema referido al centro de volumen sigue la dirección longitudinal del AirWhale, apuntando al morro de la aeronave, y el eje Z apunta hacia debajo de forma perpendicular. C (1) 17 17 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Figura 3.1. Ilustración de los sistemas de coordenadas empleados [3] En este sentido, se define el vector de estados como: 𝑞= ( 𝑥𝑦𝑧𝑝𝑞𝑟 ) Donde 𝑥, 𝑦 y 𝑧 representan la posición del centro de volumen y 𝑝, 𝑞 y 𝑟 son los giros del propio sistema. Continuando con la definición de elementos, se expondrá a continuación la equivalencia a cada uno de los términos de la ecuación (1), puntualizando antes la siguiente igualdad: 𝑒=(𝑒1 𝑒2 𝑒3); 𝑒×=(0 −𝑒3𝑒1 𝑒30 −𝑒1 −𝑒2𝑒10) Y definiendo la matriz de rotación en función de las coordenadas de Euler, que servirá para convertir las ecuaciones a coordenadas de Euler: 𝑅=( 𝑐𝑜𝑠𝜓𝑐𝑜𝑠𝜃 𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜃 −𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜓𝑠𝑖𝑛𝜃𝑠𝑖𝑛𝜙−𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜙 𝑠𝑖𝑛𝜓𝑠𝑖𝑛𝜃𝑠𝑖𝑛𝜙−𝑐𝑜𝑠𝜓𝑐𝑜𝑠𝜙 𝑐𝑜𝑠𝜃𝑠𝑖𝑛𝜙 𝑐𝑜𝑠𝜓𝑠𝑖𝑛𝜃𝑐𝑜𝑠𝜙−𝑠𝑖𝑛𝜓𝑠𝑖𝑛𝜙 𝑠𝑖𝑛𝜓𝑠𝑖𝑛𝜃𝑐𝑜𝑠𝜙−𝑐𝑜𝑠𝜓𝑠𝑖𝑛𝜙 𝑐𝑜𝑠𝜃𝑐𝑜𝑠𝜙) Así:  Matriz de masa: 𝑀𝑚𝑎𝑠𝑎=( 𝑚∙𝑰 −𝑚∙𝒓𝑮× 𝑚∙𝒓𝑮×𝑱) Donde 𝑚 es la masa del AirWhale, 𝑟𝐺 el vector de posición del origen de coordenadas (supuesto nulo), 𝑰 la matriz identidad y 𝑱 la matriz de inercia, que se ha simplificado a su forma diagonal. (2) (3) (5) (4) Modelo dinámico del AirWhale 24 24 𝒙󰇗𝒓(𝑡)= ( 𝜙𝑝(𝑡) 𝜃𝑝(𝑡) 𝜓𝑝(𝑡) 𝑝𝑝(𝑡) 𝑞𝑝(𝑡) 𝑟𝑝(𝑡) ) 𝒖𝒓(𝑡)=(𝑚𝑡𝑥𝑝(𝑡) 𝑚𝑡𝑦𝑝(𝑡) 𝑚𝑡𝑧𝑝(𝑡)) 𝒚𝒓(𝑡)=(𝜙𝑝(𝑡) 𝜃𝑝(𝑡) 𝜓𝑝(𝑡)) 𝑨𝒓= ( 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ) 𝑩𝒓= ( 0 0 0 0 0 0 0 0 0 1 𝐼𝑥𝑥 0 0 01 𝐼𝑦𝑦 0 0 0 1 𝐼𝑧𝑧 ) 𝑪𝒓=(1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0) Las ecuaciones (23  36) definen completamente los espacios de estados lineales asociados a los subsistemas de traslación y rotación y, por ende, definen el sistema linealizado completo. La simulación del sistema mostrado durante este capítulo se ha decidido incluir en el trabajo de la referencia [4]. Todo el equipo Automático ha estado realizando simulaciones activamente sobre este sistema debido a su trascendencia para todos los trabajos individuales, pero quizá sea más trascendente para la creación de los controladores PID, por lo que se ha decidido mostrar la simulación y la validación del modelo dinámico, tanto en su forma lineal como en su forma no lineal, en el trabajo de José Luis Holgado Álvarez. (31) (32) (33) (34) (35) (36) 25 25 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale 4 SELECCIÓN Y COMPRA DE LOS COMPONENTES ELECTRÓNICOS efinido el modelo dinámico que representa fielmente el comportamiento de la aeronave a construir, guardando grandes similitudes con el AirWhale definido en los trabajos del equipo Mecánico, cabe abordar qué componentes electrónicos se usarán para el vuelo de la misma. Esta electrónica hace referencia tanto a los actuadores (motores y servos), como los sensores (GPS, sónar…) y conexionado entre unos y otros (cableado, adaptadores, distribuidores de potencia…), además de la alimentación de todos ellos (baterías). Todos los componentes han sido seleccionados cuidadosamente junto con el alumno Juan Carlos Martín Rodríguez, teniendo en mente la construcción del prototipo, realizándose cambios de última hora en la misma (se indicarán en su respectivo capítulo); y de la versión final del AirWhale. En este apartado se hará una revisión de todas las compras realizadas por la asociación EsiTech y el Departamento de Ingeniería de Sistemas y Automática, justificándolas. 4.1 Plataforma ArduPilot La plataforma autopiloto fue escogida específicamente por el Departamento de Ingeniería de Sistemas y Automática. La razón fue el conocimiento de las bondades de su uso por parte del testimonio del profesor Francisco Gavilán Jiménez, que, en esos momentos, tutelaba el proyecto del alumno Juan Antonio García Fernández, estudiante del Grado en Ingeniería Aeroespacial [17]. En él, el alumno desarrollaba un sistema hardware-in-the-loop usando la plataforma ArduPilot y otros componentes para observar una simulación en vuelo. El profesor Francisco Gavilán recomendó la compra y uso de la plataforma ArduPilot, y así se hizo. El alumno que escribe este texto ha tenido que trabajar con la plataforma sin conocer previamente cómo gestiona sus recursos y cómo se programa en ella, aún entendiéndose mejor con otras plataformas igual de válidas e, incluso, más potentes. La plataforma pesa, aproximadamente, 28 gramos. Este dato es importante de cara a la construcción del prototipo y a la selección de motores y baterías, y también se indicará en los siguientes dispositivos. 4.2 Sensores Junto con la plataforma ArduPilot, se compraron los sensores externos a ella. Estos también fueron escogidos por el Departamento de Ingeniería de Sistemas y Automática por recomendación del profesor Francisco Gavilán. Estos sensores son:  3DR uBlox GPS: Un módulo que incorpora al ArduPilot un sensor GPS y un magnetómetro externo. Lógicamente, aporta al aparato un dato de posición en todo momento, siempre y cuando su vuelo sea en exterioresDispone de un puerto de 6 pines para conectar el sensor GPS a la placa (en el puerto de uso exclusivo para ello) y un puerto de 4 pines para conectar el magnetómetro (puerto I2C). D Selección y compra de los componentes electrónicos 26 26 Figura 4.1. Módulo 3DR uBlox GPS con magnetómetro incorporado La flecha que aparece en el dispositivo marca la posición en la que se debe colocar en el momento del montaje en la aeronave (eje X). Las características generales del módulo son [18]: - Módulo uBlox NEO7, con refresco de 5 Hz. - Filtros LNA (Low Noise Amplifier) y SAW (Surface Acoustic Wave) incorporados para mejorar la señal proveniente de la antena, además de un regulador de tensión de ruido bajo de 3.3 V. - Tamaño: 38x38x8.5mm. Peso: 16.8 gramos.  3DR Power Module: Su función principal no es la de sensor, sino la de suponer un puente entre la alimentación principal y la plataforma ArduPilot. Dispone de dos conectadores XT60 (hembra-macho), uno para la conexión de la/s batería/s y otro en el que se conectarán los motores mediante algún tipo de conexión casera o una placa de distribución de potencia. También cuenta con un sensor de corriente/voltaje asociado a la batería para informar de la carga restante, útil para finalizar el vuelo antes de la descarga completa de la misma y para medir la autonomía de forma fiable y directa. Figura 4.2. 3DR Power Module En concreto, dispone de las siguientes especificaciones de interés [19]: - Tensión máxima admisible de entrada: 18 V (equivalente a una batería LiPo de 4 celdas). Tensión mínima admisible: 4.5 V. Intensidad máxima admisible: 90 A (para la placa PIXHAWK, esta corriente baja a 60 A). - Proporciona 5.3 V y 2.25 A como máximo, regulados. - Puerto de 6 pines para la conexión con la plataforma ArduPilot (en el puerto destinado para ello), alimentando a la placa y aportando datos sobre la carga de la batería en tiempo real. - Tamaño: 25x21x9mm. Peso: 17 gramos. 27 27 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale  3DR Radio Set: La necesidad de la monitorización de los distintos datos administrados por la plataforma ArduPilot en vuelo supone el uso de un sistema de telemetría. El producto incluye dos módulos de radio con antenas, un cable microUSB, un cable adaptador OTG para Android y cables conectores con APM. Estas son sus especificaciones [20]: o 433 MHz de frecuencia, de acuerdo con la legislación vigente del país y de la Unión Europea. También puede trabajar a 915 MHz en adaptación con las leyes americanas. o Puerto de 6 pines para la conexión con la plataforma ArduPilot (en el puerto específico para ello). o 100 mW de potencia de salida máxima, además de una sensibilidad en la recepción de -117 dBm y el uso de la técnica TMD para la comunicación. o Conector RP-SMA para la incorporación de una antena. o Modulación FHSS, interfaz UART, soporte del protocolo Mavlink y configurable mediante Mission Planner. o Voltaje de alimentación 3.7-6 V en continua, corriente en transmisión de 20 mA a 20 dB, corriente en recepción de 25 mA y tensión en interfaz UART de 3.3 V. o Tamaño: 26.7x55.5x13.3mm. Peso:  20 gramos. Figura 4.3. 3DR Radio Set  Sónar XL-MAXSONAR-EZ/AE MB1240: Este sensor se usa frecuentemente para la monitorización de la altitud (dirigiéndolo al suelo) o para la detección de obstáculos. Al disponer la plataforma ArduPilot de un barómetro que calcula automáticamente la altitud a partir de la presión, la utilidad de este sensor, en caso de emplearse, será la de detección de elementos con los que la aeronave pueda chocar. Selección y compra de los componentes electrónicos 28 28 Figura 4.4. Sónar XL-MAXSONAR-EZ MB1240 Algunas de sus especificaciones son [21]: o Resolución de 1cm y lectura a 10 Hz. o En general, los objetos a distancias menores de 25 cm se detectan a 25 cm. o Actúa a 3.3-5.5V y a 3.4 mA, y se puede leer la distancia, directa o indirectamente, de tres salidas propias del sónar: voltaje analógico, en forma de puerto serie con formato RS232 y PWM, realizando ajustes en los pines del sensor. o Rango de funcionamiento de temperaturas de 0 a 65ºC, calibración automática en tiempo real y filtrado. o Detección de obstáculos a una distancia máxima de 765 cm. o Tamaño: 22.1x19.9x25.1mm. Peso: 6.1 gramos. 4.3 Almacenamiento Para almacenamiento de datos en vuelo, viene incluido en las compras realizadas por el Departamento un dispositivo de almacenamiento USB: DataTraveler microDuo 3.0, de Kingston Technology. Se caracteriza por [22]:  32 GB de almacenamiento.  70 MB por segundo de lectura y 15 MB por segundo de escritura.  Interfaz compatible con USB 3.0 y microUSB.  Tamaño: 27.63x16.46x8.56mm. Peso: 45.36 g. Figura 4.5. Dispositivo de almacenamiento DataTraveler microDuo 3.0 29 29 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale 4.4 Alimentación Las baterías son uno de los componentes más importantes del proyecto, debido al objetivo principal del mismo: aumentar la autonomía de los UAV actuales. Las baterías LiPo que se pueden encontrar en el mercado están, generalmente, clasificadas según la tensión que aportan a la salida de sus bornes, y este parámetro determina el número de celdas que poseen (cada celda posee un voltaje de 3.7 V). A su vez, la tensión determina el regulador y el motor que se van a conectar a dicha batería. Este último factor se debe unir a los ya importantes parámetros del peso y la capacidad de la batería. Por ello, hay que llegar a una solución de compromiso, siempre teniendo en mente conseguir la mayor autonomía posible. En primera instancia, repasemos la capacidad de la batería. Normalmente ésta se da en miliamperios por hora (mAh) y es el factor más determinante en relación a la autonomía. Cuanta más capacidad tiene la batería, mayor autonomía de vuelo. Hay multitud de baterías en el mercado, todas con una gran variedad de capacidades, desde 600 mAh (más baratas) hasta 6000 mAh (más caras). Hay valores en el mercado que se salen de este rango, pero abandonan los precios y la autonomía deseados, por lo que no son de interés en este proyecto. Conforme aumenta la capacidad de la batería, generalmente también lo hace el peso. Afortunadamente, como se verá en la siguiente sección, los elementos estructurales y la electrónica presentada anteriormente no suponen un gran aumento del peso de la aeronave, y el helio lo contrarresta completamente, por lo que hay margen para seleccionar baterías de gran capacidad aunque su peso sea medianamente alto. Bajo estas premisas, y después de buscar detenidamente en el mercado, se han seleccionado las baterías Zippy 5000mAh 3S (11.1V) 20C-30C. Figura 4.6. Batería Zippy 5000mAh 3S 20C-30C Por el nombre, puede saberse que posee una capacidad de 5 amperios por hora y proporciona 11.1 V a la salida (3S = tres celdas). El último término, 20C-30C, hace referencia al índice de descarga. El primer término (20C) hace referencia a la intensidad de descarga constante máxima que es capaz de proporcionar la batería. Es decir, en este caso: 20·5000mA = 100000mA = 100 A. Este dato determina que la batería puede proporcionar una corriente constante de 100 A, si el sistema así lo requiriese. Este índice, por tanto, debe ser alto, y normalmente guardar un margen de seguridad del 50% al 100%. El segundo término (30C) se refiere a la intensidad de descarga puntual máxima que puede proporcionar la batería. En este caso: 30·5000mA = 150000mA = 150 A, es decir, de forma puntual y esporádica, la batería podría llegar a dar una intensidad de descarga de 150 A. Las dimensiones de la batería son 138x47x25mm y tiene un peso aproximado de 300 gramos. Selección y compra de los componentes electrónicos 30 30 4.5 Actuadores Los dispositivos electrónicos actuadores, aquellos que llevarán a cabo las fuerzas y pares necesarios para el control óptimo de la aeronave, se seleccionan, principalmente, a partir de un dato clave: el peso de la estructura. Es claro que, si lo que se desea es que algún vehículo vuele, cuánto más pese, mayor debe ser la fuerza que ejercen los actuadores para conseguir el vuelo. En una primera aproximación, dado que la intención es que los actuadores que se seleccionen también puedan emplearse en la versión final del AirWhale, se va a partir del dato del peso de la estructura reflejado en los cálculos del equipo Mecánico-Aeronáutico y de la electrónica anteriormente presentada, además de la que está por presentar. En cuanto a la estructura del AirWhale, mediante los trabajos del equipo Mecánico-Aeronáutico, se pueden recoger los siguientes pesos [2]:  Peso del cuerpo y estabilizadores: 1.3 Kg.  Peso de las alas: 0.3 Kg. Además, la alumna Inmaculada Gómez calcula en su trabajo [2] la sustentación que ejerce el volumen de helio confinado por la estructura: 2.11 Kg. Esto, contando sólo la estructura propia del AirWhale, sin añadir en la ecuación el peso de la electrónica, deja un balance negativo de 510 gramos. Es decir, olvidando la electrónica, la estructura volaría por sí misma gracias a la acción del helio. Ahora se reflejará el peso de la electrónica presentada hasta ahora, esto es: plataforma autopiloto, sensores, dispositivos de almacenamiento y baterías:  ArduPilot: 28 gramos.  GPS: 16.8 gramos.  Módulo de alimentación: 17 gramos.  Telemetría: 20 gramos.  Sónar: 6.1 gramos.  Dispositivo almacenamiento USB: 45.36 gramos.  4 baterías Zippy 5000 mAh 3S 20-30C (se decidio usar una por cada motor): 1200 gramos. Lo que hace un total de 1333.46 gramos. Esto, en el balance general, supone un total de 823.46 gramos a levantar por los motores, sin contar el peso de los mismos y de los variadores/reguladores que les acompañan, de los cables y de otros componentes. Para el control de la aeronave, hay que recordar que son necesarios cuatro motores, dos verticales y dos horizontales, y cuatro servomotores (con control de posición incorporado) para los cuatro estabilizadores mostrados en el diseño final. Posteriormente, en el montaje del prototipo, se podrá comprobar cómo el número de servomotores se reduce a dos, por simplicidad y para habilitar el control del cabeceo de la aeronave. 4.5.1 Motores principales Empecemos seleccionando los motores. Una vez escogidas las baterías y determinado el peso que hay que levantar, es fácil encontrar un motor adecuado para cumplir la misión de este trabajo. De las baterías se sabe que los motores deben funcionar a una tensión máxima de 11.1 V, en concordancia con la máxima que puede aportar una batería de las seleccionadas. En cuanto al peso, se sabe que, mínimo, se necesitarán levantar aproximadamente 825 gramos, cantidad que aumentará con el peso de los motores escogidos y demás componentes. Por ello, hay que buscar un motor poco pesado que no aumente demasiado el peso total de la estructura, que ejerza una fuerza suficiente como para levantar la aeronave y que consuma poca potencia, de manera que no exija demasiado a las baterías y no afecte considerablemente a la autonomía. Sabiendo todos estos factores, se decide adquirir el motor Brushless outrunner EMAX BL2210/30 - 1300kV, mostrado en la Figura 4.7. 31 31 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Figura 4.7. Motor Brushless EMAX BL2210/30 Las características de interés de este motor se resumen en:  Tensión de entrada: 7.4-11.1 V.  Consumo: 166 W. 1300 RPM por voltio.  Peso: 45 gramos.  Corriente máxima: 16.5 A. El 75% de esta corriente da la máxima eficiencia.  Empuje máximo: 800 gramos (implementando hélices de 9x4.7 pulgadas). Dados los datos anteriores, añadir estos cuatro motores a la estructura supone una sustentación máxima de 1600 gramos y un peso extra de 180 gramos. En total, una estructura de 1003.46 gramos totalmente cubiertos por la acción de los motores. 4.5.2 Hélices propulsoras Los motores seleccionados anteriormente no podrían elevar ninguna aeronave sin acoplarles hélices que ejerzan la fuerza necesaria de sustentación. Para seleccionar qué tipo de hélices son las correctas con el tipo de motor seleccionado, el propio fabricante ofrece recomendaciones sobre las dimensiones de las mismas, añadiendo el empuje que generaría el tándem motor-hélice. En el caso de emplear una hélice de tamaño 9x4.7 pulgadas, el motor seleccionado aporta el empuje máximo de 800 gramos, así que será éste el tamaño buscado. Con respecto al eje de la hélice, sería deseable que guardase gran similitud con el diámetro del eje del motor, pero puede no ser necesario dependiendo de la conexión usada entre hélice y motor. Con los motores se incluyen gomillas, elemento que permite una unión que no necesita imprescindiblemente la semejanza de ejes. Por último, resta decidir el material de las hélices. Existe varios materiales: madera, plástico ABS, fibra de carbono… Debido a su rigidez, que supone resistencia a vibraciones, y poco peso, se ha optado por hélices de fibra de carbono. Las hélices escogidas son de fibra de carbono, 9x4.7 pulgadas y eje de 8mm. Cada una pesa 10 gramos, y al añadir una por cada motor, también se añaden 40 gramos más a la estructura: 1043.46 gramos en total. Se hará uso de hélices rotatorias y contrarotatorias. Selección y compra de los componentes electrónicos 32 32 Figura 4.8. Hélice 9x4.7 de fibra de carbono. 4.5.3 Servomotores Los servomotores, por otro lado, deben ser capaces de soportar la fuerza que pueda ejercer el aire en la superficie de control, además del propio peso de la misma, y es conveniente que consuman lo menos posible. Los servomotores mostrados en el mercado suelen venir acompañados de dos parámetros principales: el ‘torque’ y la velocidad de actuación, y cada uno de ellos depende de la tensión de alimentación que reciba el servo. El ‘torque’ es una medida referida a la fuerza que puede ejercer el servomotor. Concretamente, se mide en kilogramos por centímetro (Kg/cm), e indica los kilogramos de fuerza que ejerce el servo al girar al final del brazo por centímetro de brazo, asemejándose al concepto tradicional de momento. Conviene, por tanto, que sea lo más alto posible. La velocidad de actuación es un concepto más intuitivo; se mide en segundos por cada sesenta grados (s/60º) e indica el tiempo que tarda el servomotor en moverse 60 grados. Este dato determina completamente la velocidad del servo en su giro, e interesa que sea lo más rápido posible, sobre todo si de estos dispositivos depende el control de alguna variable de estado de la aeronave. El voltaje al que se alimenta el servomotor influye en lo anterior. En las especificaciones facilitadas por los fabricantes, suelen indicar el ‘torque’ y la velocidad de actuación a dos voltajes comunes: 4.8 V y 6.0 V. En nuestro caso, como no es posible alimentar directamente los servomotores desde las baterías (debido a su alto voltaje), se alimentarán directamente desde la placa. El servomotor seleccionado es el TowerPro SG92r 9G, que dispone de las siguientes características:  ‘Torque’: 1.5 Kg/cm a 4.7 V, 1.6 Kg/cm a 6 V.  Velocidad de actuación: 0.12 a 4.7 V.  Rango de voltaje admitido: 3.0-7.2 V.  Tamaño: 22x11.5x27mm. Peso: 9 gramos. Lo que supone añadir a la estructura 36 gramos más: 1079.43 gramos en total. 33 33 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Figura 4.9. Servomotor TowerPro SG92r 9G 4.6 Variadores/reguladores Los variadores/reguladores son dispositivos que se encargan de variar la velocidad del motor desde su mínimo hasta su máximo, y de transformar la corriente continua que le llega desde la batería a trifásica, que es la que solicita el motor. Además, supone la conexión de las salidas PWM de la placa con el motor. Estos dispositivos son muy variados y pueden disponer de multitud de características, algunas dependientes de la batería a la que se conecten y otras del motor al que alimenta. La intensidad que ofrecen estos dispositivos depende exclusivamente de la intensidad máxima que aceptan los motores a los que están conectados. Es recomendable guardar un margen de seguridad con respecto a este valor para seleccionar el variador, siendo frecuente que el fabricante del motor recomiende el tipo de variador a usar. En el caso de nuestro motor, se recomienda un variador con una intensidad continua de 18 A como mínimo. Existe un amperaje máximo adicional que puede proporcionar el variador de forma puntual, mayor que el valor continuo. Normalmente, el fabricante del variador también indica qué tipo de batería recomienda conectar con el mismo, concretamente determinando el número de celdas (voltaje) que debe caracterizar a la batería. Aparte de estos parámetros, los variadores pueden disponer de BEC (Battery Elimination Circuit). Es, básicamente, un regulador que obtiene la tensión e intensidad provenientes de la batería y los convierte en un voltaje y amperaje estables, de manera que puede llegar a alimentar elementos que requieran poca potencia para funcionar. Esta alimentación acompaña al cable de señal proveniente de la placa, y es posible usarla para la alimentación de la plataforma ArduPilot, como se detallará en la construcción del prototipo y conexionado. Se optará por un variador que incluya este sistema de alimentación, en pro de la versatilidad que ofrece. El seleccionado es el regulador Brushless EMAX 18A Budget, mostrado en la Figura 4.10. Figura 4.10. Regulador Brushless EMAX 18A Budget Firmware ArduCopter y creación de un firmware para la aeronave AirWhale 40 40 programación relacionada con los pines del microprocesador. La última versión estable del código ArduCopter a estudiar se puede descargar íntegramente desde el repositorio oficial del Proyecto ArduPilot, concretamente desde el enlace [23]. 5.1.1 Librerías Como se ha apuntado, existe un gran número de archivos que constituye la librería común a todos los firmwares. Este gran número es el resultado de la necesidad de cubrir multitud de tareas que aparecen en cada uno de los firmwares específicos y la definición de funciones necesarias para la compatibilidad con otras plataformas autopiloto. A continuación se enumerarán y detallarán brevemente cada una de ellas, dejando una explicación más profunda de las librerías útiles, para el código a construir, para secciones posteriores. 5.1.1.1 Definición general de librerías Se presentarán las librerías según el nombre de la carpeta que las engloba:  AC_AttitudeControl: Este conjunto de librerías introduce variables y funciones relacionadas con el control de la orientación y posición de la aeronave, sea cual sea el firmware. Esta relacionado directamente con los archivos ‘control_X.pde’ del firmware ArduCopter. Para el objetivo de este trabajo, no van a ser necesarias, ya que se programarán las leyes de control desarolladas por el equipo Automático, y por tanto tampoco se hará uso de los archivos ‘control_X.pde’. También incluyen librerías específicas para el control de la orientación y la posición del helicóptero, el cual consta de particularidades lo suficientemente importantes como para el desarrollo de archivos específicos para suplirlas.  AC_Fence: Una librería que define límites geométricos al vuelo de la aeronave, tanto en altitud como distancia al punto inicial o ‘home’. Concretamente, se define una circunferencia límite en el plano XY en la cual la aeronave puede operar sin problemas, y valor de altitud límite. Cuando la aeronave supera estos límites, el firmware cambia el estado de la aeronave o la función a realizar en pro de la seguridad. Muy útil en caso de arrastre por causa del viento. No se usará debido a que no se realizarán pruebas en exteriores, y el vuelo será en un entorno controlado.  AC_PID: Librerías que incluyen las funciones que definen los algoritmos de los controladores proporcionales (P) e integrales y derivativos (PID), y las particularizaciones para el helicóptero. No se usará, es preferible reprogramarlo.  AC_Sprayer: Librería que se encarga del control del ‘crop sprayer’ o rociador de cultivos, si la aeronave lo tuviese. No se usará, por motivos obvios.  AC_WPNav: Conjunto de librerías que contienen las funciones y variables necesarias para la correcta navegación de la aeronave a partir de balizas o ‘waypoints’. También incluyen lo propio para una navegación en círculo. Aunque es interesante dotar a la aeronave de la posibilidad de la navegación por ‘waypoints’, en esta primera versión de código no se implementará. En líneas de trabajo posteriores se podría abordar esta tarea.  AP_ADC y AP_ADC_AnalogSource: Estas librerías contienen el código específico para el uso de los convertidores analógico-digitales que pueda usar la placa Ardupilot. Se ha observado el uso de esta librería en el código particular ArduCopter y no se encuentran grandes usos. Parece ser necesario si se dispone de una placa APM 1, pero al poseer una plataforma APM 2.6, no se tendrá en cuenta.  AP_AHRS: Estas librerías contienen los algoritmos necesarios para implementar el llamado ‘Attitude and Heading Reference System’ (AHRS). Este tipo de sistemas proporciona datos sobre la orientación, el rumbo y la posición de la aeronave. Esto se consigue gracias a la fusión de la medida de giróscopo, acelerómetro y magnetómetro (en algunos casos, también GPS) mediante la aplicación de algún tipo de 41 41 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale filtro. En el caso de esta librería, se usan dos filtros: el famoso Filtro de Kalman Extendido (EKF), y otro basado en el DCM (Direction Cosine Matrix). Según la documentación encontrada en la web oficial del Proyecto ArduPilot [24], el filtro de Kalman no puede aplicarse en ninguna de las versiones de la plataforma ArduPilot, por el momento, debido a su reducida capacidad de cálculo.  AP_Airspeed: Aportan los archivos necesarios para la implementación de funciones y variables clave para el uso correcto y calibración de sensores de velocidad del aire alrededor de la aeronave, todo ello para distintas placas autopiloto. Dado que la aeronave a construir no operará a velocidades muy altas, se ha prescindido tanto del sensor como de las librerías asociadas.  AP_Arming: Define algunas funciones de comunicación con la estación en tierra de verificación, permitiendo o no el armado final de la aeronave según la comprobación de aspectos relacionados con los sensores, radio y otros. En principio, no será necesario este conjunto de archivos.  AP_Baro: Aporta las funciones necesarias para manejar los sensores barométricos compatibles con las distintas plataformas disponibles. Estas funciones incluyen tareas de inicialización del barómetro, obtención de datos proporcionados por el sensor (temperatura y presión), su calibración y demás acciones.  AP_BattMonitor: De forma análoga a la anterior librería, este conjunto define las funciones para gestionar los datos obtenidos por el sensor que incluye el 3DR Power Module.  AP_BoardConfig: Relativo a opciones varias sobre pines y variables relacionadas con las plataformas autopiloto PX4, PIXHAWK Y VBRAIN. No se detallará.  AP_Buffer: Librería que aporta funciones para manejar filas o colas de datos, tales como la definición de la cola, adición y sustracción de datos, identificar el orden del dato o conocer el número de elementos de la fila.  AP_Camera: Librería relacionada con las acciones realizables con la cámara que pueda añadirse a la aeronave. En una primera versión del firmware, no se incluirá.  AP_Common: Definición de funciones comunes, usualmente utilizadas, como la conversión de grados a radianes, y viceversa; fijar la velocidad de transmisión de datos por el puerto serie y funciones para eliminar errores y avisos comunes del compilador. Las funciones de conversión de ángulos serán usadas con asiduidad, aunque sean fáciles de reprogramar. También incluye declaraciones relacionadas con testeos del firmware y compatibilidad Arduino-C++.  AP_Compass: Incluye todas las funciones necesarias para la obtención y gestión de datos proporcionados por los modelos de magnetómetro admitidos por las placas disponibles. En el caso del AirWhale, se dispone de un magnetómetro junto al módulo de GPS, contemplado por la biblioteca.  AP_Curve: Librería que aporta las funciones necesarias para definir la curva que relaciona los valores PWM con la fuerza de sustentación del motor relacionado (añadir/eliminar puntos a/de la curva, cálculo de los valores de PWM o de la fuerza de los motores por aproximación lineal…). Es usada frecuentemente por el firmware ArduCopter, y es clave en la definición de los motores y los PWM a mandar a cada uno. No se va a usar, ya que se realizará aparte un experimento para encontrar esa relación necesaria para el control óptimo de la aeronave.  AP_EPM: Biblioteca que permite la gestión y uso adecuado del EPM_CargoGripper, un dispositivo magnetoelectrónico que ejerce un campo magnético para el agarre de cargas metálicas de peso máximo de un kilogramo [25]. Como este dispositivo no se usará, se prescinde de esta librería.  AP_Frsky_Telem: Aporta variables y funciones que realizan acciones relacionadas con la telemetría conectada a la placa disponible, en su mayoría de comunicación.  AP_GPS: Conjunto de bibliotecas que habilitan distintos modelos de GPS. Aunque se hará uso del GPS sólo porque el estimador que se usará para la obtención de los ángulos de navegación lo necesita, no se hará la lectura del mismo de forma directa. Las funciones que se usarán se definirán en su momento.  AP_HAL: Uno de los conjuntos de librerías más importantes del firmware. Se encarga de la definición Firmware ArduCopter y creación de un firmware para la aeronave AirWhale 42 42 de las variables y funciones relacionadas con la plataforma autopiloto en sí: lectura, definición y escritura en pines de entrada y salida, puertos SPI, I2C, almacenaje en memoria, gestión de tiempo de cómputo de distintas tareas, comunicación por puerto serie y consola del IDE y muchas más otras tareas. Ampliamente usada por el firmware y, evidentemente, indirectamente por el código creado para el AirWhale, pero no se entrará en detalles sobre las distintas librerías que se encuentran aquí debido a su gran extensión. Las funciones que aparezcan posteriormente relacionadas con estas bibliotecas serán definidas para su fácil comprensión.  AP_HAL_AVR, AP_HAL_AVR_SITL, AP_HAL_Empty, AP_HAL_FLYMAPLE, AP_HAL_Linux, AP_HAL_PX4, AP_HAL_VRBRAIN: Todas estas bibliotecas se encargan de lo mismo que el conjunto AP_HAL, pero para distintas placas. En este sentido, no se explicarán. En conjunto, suponen el mayor número de archivos de librería de todos.  AP_InertialNav: Al igual que la biblioteca AP_AHRS, este conjunto incluye algoritmos necesarios para obtener datos precisos sobre la posición y altitud de la aeronave, fusionando datos de los sensores acelerómetro, GPS y barómetro. Existen particularidades dependiendo del método de integración y fusión de sensores empleados (DCM o EKF).  AP_InertialSensor: Funciones necesarias para gestionar correctamente el sensor acelerómetro y giróscopo integrados en la placa APM 2.6.  AP_L1_Control: Biblioteca necesaria para el control adaptativo L1. No se entrará en detalles, ya que no es cometido de este trabajo.  AP_Limits: Impone límites a la localización de la aeronave imponiendo ‘geovallas’, tal y como pudo verse en AP_Fence; y otros parámetros susceptibles a restricciones relacionados con el GPS o la altitud. No se usará.  AP_Math: Bibliotecas que introducen funciones matemáticas comunes y otras más elaboradas, permitiendo el uso de vectores, matrices, operaciones de conversión de unidades, trigonometría, cambio de variables, geometría y muchas otras más tareas. No se entrará en más detalles, salvo cuando se deba detallar más adelante.  AP_Motors: De gran importancia en el firmware, define los parámetros y aporta las funciones para la definición y gestión de los motores en el código. Contempla distintas disposiciones de los motores (quadrotor en X o en cruz, hexarotor…) e incluye algoritmos para la definición del valor de PWM que debe recibir cada motor según los valores que deberían tener las variables de estado de la aeronave.  AP_Mission: Esta librería lee y escribe comandos relacionados con las misiones que se definen en Mission Planner, además de permitir el fácil acceso a parámetros relacionados con los ‘waypoints’. Como se dijo anteriormente, se va a prescindir en esta primera versión del firmware de la operación de vuelo por balizas.  AP_Mount: Librería para el control del montaje de la cámara u otro dispositivo con un montaje de dos o tres ejes móviles. No se detallará.  AP_NavEKF: Definición de las variables y funciones que hacen posible la aplicación del filtro de Kalman extendido (Extended Kalman Filter). Debido a la cantidad de recursos a nivel de procesador que requiere la aplicación de este filtro, no puede utilizarse bajo la placa ArduPilot. Su programación está dirigida a la placa PX4/PIXHAWK.  AP_Navigation: Declaración de funciones relacionadas con la navegación por ‘waypoints’, como la modificación de ángulos de orientación de la aeronave con respecto a las balizas, la aceleración lateral de la aeronave en movimiento a la baliza o la comunicación con las librerías AC_WPNav, actualizando el estado de controlador de navegación dados el anterior y próximo ‘waypoint’. No se detallará en profundidad.  AP_Notify: Gestión de la iluminación de los leds de la placa en función de la notificación que se quiera dar al usuario, además de la definición de variables de estado que propinan el aviso de distintos eventos (GPS mal medido, firmware armado, fallo del filtro de Kalman programado, activaciones del modo seguro de un sensor dado…). 43 43 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale  AP_OpticalFlow: Se encargan del funcionamiento adecuado del sensor de flujo óptico, si lo hubiese. Este sensor es capaz de medir la velocidad de movimiento de la aeronave comparando el movimiento de un fotograma captado en un instante con respecto al mismo fotograma en un instante justo anterior. Como no se usará, esta biblioteca no es de interés en este trabajo.  AP_Parachute: Librería encargada del funcionamiento de un paracaídas, si la aeronave dispusiese de uno. No es el caso, por ello no se detallará más.  AP_Param: Otro de los conjuntos más importantes. Se encarga de la gestión y definición de variables comúnmente y ampliamente usadas por el firmware. Son usadas grandes estructuras de datos para guardar las variables necesarias, y son susceptibles de ser parámetros de funciones de escritura, copia, búsqueda, modificación, obtención de información y demás tratamiento de variables. Debido a su gran extensión, no se entrará en más detalle, aunque las funciones a usar se basen en variables y funciones de estas bibliotecas.  AP_PerfMon: Define funciones para la captura de tiempos de cómputo de funciones basándose en funciones más simples de captura de tiempos. Estos datos se guardan en una lista de variables temporales que serán manipuladas por otras funciones descritas. No se usará.  AP_ProgMem: Observando los archivos, se puede comprobar que aporta funciones relacionadas con la captura del tamaño de variables de distintos tipos, funciones para conocer la longitud de cadenas de caracteres, funciones de copia de cadenas y demás funciones sencillas. Existen funciones definidas en esta biblioteca que aparecen en el envío de PWM a los motores (se indicará en su momento).  AP_Rally: Incorpora al firmware la capacidad de crear puntos virtuales geométricos al que la aeronave acudiría en caso de que active su estado RTL (Return To Launch), normalmente por una decisión en pro de la seguridad. Es útil cuando la vuelta a la posición ‘home’ (acción por defecto en el modo RTL) resulta ser una operación comprometida.  AP_RangeFinder: Bibliotecas que definen funciones para gestionar las medidas proporcionadas por los distintos modelos de sónar más usados o soportados por las placas que soportan el firmware. Interesante su uso, ya que se dispone, como se vio en el Capítulo 4, de un sensor sónar. Sin embargo, en una primera versión del firmware, no será objetivo principal el uso del sónar, y se pospone para versiones posteriores.  AP_RCMapper: Necesaria para la conversión de las entradas de radiocontrol en empuje, par en roll, pitch y yaw. En un principio, como no se hace uso de un radiocontrol ni de un receptor en los pines de entrada, no se detallará.  AP_Relay: Define el driver para los pines usados como pines ‘relay’: pines digitales que proporcionan 0 V o 5 V (en el caso de la placa APM). Según el código, un máximo de cuatro pines se pueden definir como ‘relays’, cualquiera de los pines A0-A8 de la placa. Puede ser útil para la alimentación de servomotores. No se usará en esta primera versión.  AP_Scheduler: Se encarga de la gestión eficaz del tiempo del que dispone cada proceso para completarse y el periodo de repetición del mismo mediante un arcaico sistema de ejecutivo cíclico, ya que la placa APM 2.6 no soporta el ‘threading’.  AP_ServoRelayEvents: Relacionado con el anterior conjunto de librerías; define funciones para mandar señales a través de los pines de ‘relay’, concretando según el dispositivo conectado a ellos.  AP_SpdHgtControl: Una interfaz genérica para todos los controladores de velocidad y altitud. Define funciones generales a ellos, y supone la clase común a todos. Luego, para cada controlador, existirá una clase específica basada en la definida en esta librería. Al usar controladores propios, no se detallará.  AP_TECS: Librería bastante extensa que introduce las funciones necesarias para un control combinado de la altura y la velocidad, usando el empuje total para controlar la energía total y el ángulo ‘pitch’ para repartir esa energía entre potencial y cinética, según la prioridad. Supone una subclase de la definida en la anterior librería. No se detallará por los mismos motivos anteriores.  AP_Terrain: Librería que aporta funcionalidades dirigidas a conocer el estado del terreno que se sobrevuela. Lleva a cabo tareas de reconocimiento de altitud en la posición actual sobre el terreno, Firmware ArduCopter y creación de un firmware para la aeronave AirWhale 44 44 velocidad relativa, diferencia de parámetros del terreno con respecto al encontrado en ‘home’, almacenaje de datos y reconocimiento de parámetros relacionados con el terreno y con los ‘waypoints’ y puntos ‘rally’.  AP_Vehicle: Proporciona parámetros específicos para cada tipo de vehículo (en este caso, vehículos con ala o multicópteros), además de otras variables necesarias para la selección de directorios de compilación.  APM_Control: Habilita la función ‘AutoTune’, que se encarga de actualizar cada 10 segundos las ganancias de los controladores PID usados para el control de la aeronave. Dispone de una clase general (AP_AutoTune) y clases más concretas relacionadas con cada variable a controlar (p.e: AP_YawController).  AP_OBC: Se encarga de aportar funciones de seguridad, de tal manera que si la aeronave supera los límites geométricos establecidos por AP_Fence y AP_Limits, algún sensor crítico deja de funcionar correctamente (GPS, barómetro) o se pierde comunicación con la estación de tierra, se activa el modo seguro (Failsafe).  AP_PI: Define algoritmos para la definición de un controlador PI.  Dataflash: Librería que define funciones necesarias para operar con la placa de almacenamiento que incluye la plataforma ArduPilot. Muy útil para guardar datos que en el momento no puedan leerse, pudiendo extraerlos posteriormente. Recordar que la placa de almacenamiento dispone de un total de 4 MB, por lo que es posible obtener una gran cantidad de datos de vuelo sin requerir su lectura hasta el fin del mismo.  Filter: Librerías donde se definen los distintos filtros que se usarán en las demás bibliotecas que la necesiten (en su mayoría, las relacionadas con la obtención de datos provenientes de los sensores). En total, reúne cuatro tipos de filtros: derivativo, paso bajo, paso bajo de segundo orden y filtro de Butterworth.  GCS_Console y GCS_MAVLink: Librerías que se encargan de la comunicación efectiva con la estación de tierra y el control mediante la misma, todo ello bajo el protocolo MAVLink.  RC_Channel: Conjunto de bibliotecas que se dedican a gestionar todo lo relacionado con los puertos de entrada y salida, es decir: los relacionados con la entrada de datos por el receptor de radio y la salida de PWM dirigida a los motores. Contienen funciones limitadoras de los valores a enviar, definiciones de pines, etc.  SITL: Introduce el sistema llamado ‘Software In The Loop’, que permite gestionar eficientemente los datos relacionados con la simulación de sensores por parte del simulador que esté realizándolo. Unido estrechamente con el sistema HIL.  StorageManager: Gestión de la memoria de la placa, que incluye definir los sectores de la misma para el número de ‘waypoints’, de parámetros, de puntos ‘rally’ y de puntos que definan los límites geométricos. 5.1.1.2 AP_Baro El sensor barómetrico es imprescindible para conocer la altura a la que se encuentra la aeronave, ya que utiliza la medida realizada de presión y temperatura para calcular datos como la altitud o, indirectamente, la densidad del aire. El sensor a utilizar es el incluido en la plataforma ArduPilot, el MS5611-01BA03 de Measurements Specialties, y el fabricante ofrece documentación suficiente para la conexión y tratamiento de dicho sensor [26]. En la biblioteca referente al barómetro, se puede encontrar una librería específica para el conexionado y correcta gestión del sensor MS5611, y se usará de forma íntegra, definiendo detalladamente las funciones que aparezcan explícitamente en el código a desarrollar. 45 45 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale AP_Baro.h/.cpp: Descripción y funcionamiento general El conjunto incluye una librería general llamada AP_Baro.h, y su archivo .cpp asociado, donde se define una clase pública general llamada AP_Baro que incluye funciones relacionadas con la calibración, cálculo de altitud, velocidad relativa en Z y otros. Estas funciones usarán otras definidas en las sucesivas subclases que aparecen en las librerías específicas para cada modelo de barómetro (se verá a continuación). En ningún momento, esta librería general define cómo obtener medidas del sensor, esta tarea se detalla en las librerías específicas, ya que depende de cada modelo. AP_Baro.h/.cpp: Funciones Las funciones que aparecen en AP_Baro.h/.cpp son:  void calibrate (void): Como su nombre indica, se encarga de la calibración correcta del sensor de presión. El funcionamiento es sencillo: se toma una medida inicial de la presión y la temperatura (valiéndose de las funciones definidas en las librerías específicas), que se guardarán como valores correspondientes al ‘suelo virtual’ para el firmware. Después, se mantiene el sensor tomando medidas inútiles durante un segundo. El motivo es que, precisamente, el modelo MS5611 toma medidas con un error de aproximadamente un metro durante el primer segundo de funcionamiento. Por último, se toman cinco medidas para asegurar una buena calibración, realizando la siguiente ponderación después de cada una: 𝑣𝑎𝑙𝑜𝑟𝑠𝑢𝑒𝑙𝑜=(𝑣𝑎𝑙𝑜𝑟𝑠𝑢𝑒𝑙𝑜·0.8)+(𝑣𝑎𝑙𝑜𝑟𝑚𝑒𝑑𝑖𝑑𝑜·0.2) El valor final del calibrado se obtendrá después de la quinta medida, finalizando la función.  void update_calibrate (void): Ignora el proceso anterior y guarda como valores de ‘suelo’ los obtenidos en ese momento, lo cual resulta útil para mantener actualizada la calibración del dispositivo.  float get_altitude_difference (float, float) const: Esta función constante (no puede modificar variables) realiza el cálculo de la diferencia entre la altitud en función de una presión dada (segundo parámetro) y la altitud en función de una presión base (primer parámetro), y devuelve como una variable float. Es la función que se usará mayoritariamente para calcular la altitud de la aeronave, teniendo como segundo parámetro la presión medida al momento y como primero la presión guardada a nivel de ‘suelo’. En ella, se realizan varios cálculos, dependiendo de la plataforma que se disponga. En el caso de la plataforma APM 2.6 (velocidad de 16 MHz), el cálculo realizado es el siguiente: 𝑎𝑙𝑡𝑖𝑡𝑢𝑑=𝑙𝑛(𝑝𝑟𝑒𝑠𝑖𝑜𝑛𝑏𝑎𝑠𝑒 𝑝𝑟𝑒𝑠𝑖𝑜𝑛𝑚𝑒𝑑𝑖𝑑𝑎)·𝑡𝑒𝑚𝑝𝑒𝑟𝑎𝑡𝑢𝑟𝑎·29.271269 Buscando, se ha identificado este cálculo con el cálculo de la presión a partir de la altura y la temperatura suponiendo atmósfera estándar, dado por la ecuación (1): 𝑝=𝑝0·𝑒−𝑔 𝑅𝑇·(ℎ−ℎ0) Donde 𝑝 y 𝑝0 son las presiones medida y base, respectivamente, 𝑔 es la gravedad, 𝑅 es la constante universal de los gases expresada en 𝑚2 𝑠2·𝐾 , 𝑇 es la temperatura medida y ℎ y ℎ0 son la altura a calcular y la altura inicial (nula, considerada a nivel de suelo), respectivamente [27]. Esta función devolverá cero si no ha habido una calibración previa. (1) Firmware ArduCopter y creación de un firmware para la aeronave AirWhale 46 46  float get_altitude (void): Realiza automáticamente la medición de un valor de presión en el instante en el que se ha usado, llama a la función get_altitute_difference, pasándole como parámetros el valor medido y el valor de presión base; y devuelve el valor de la altitud calculada. También realiza funciones de comprobación, como, por ejemplo, si el valor para la presión base es distinto de cero (en caso contrario, la función se habría llamado sin una calibración previa) o si realmente existe una nueva medida a tratar. Para esta última comprobación, se utilizan dos variables temporales que van guardando el momento actual de lectura de un valor del sensor y el anterior; si los valores de estas dos variables coinciden, conlleva que no ha existido lectura (no se ha actualizado la variable) y la función acaba. Existen otras dos funciones relacionadas con el cálculo de la velocidad estimada, a través de la variación de la presión teniendo en cuenta las últimas medidas empleando un filtro derivativo, pero no se usarán y, por tanto, tampoco se entrará en más detalle. AP_Baro_MS5611.h/.cpp: Descripción y funciones Esta librería, subordinada a la anterior, define las funciones necesarias para gestionar de forma correcta el modelo de sensor barómetrico del que se dispone, el MS5611. Todas las funciones realizan tareas a bajo nivel, por lo que no se entrará en mucho detalle; se proporcionará una descripción general de las más trascendentes, a modo de introducción y con el objetivo de entender qué tarea desempeña cada función. Estas funciones son:  void init (void): Habilita el puerto serie y lo asigna al barómetro. Esto puede llevar a problemas, ya que el acelerómetro y el barómetro comparten puerto SPI. Para inicializarlo, se debe asignar el puerto a los dos de forma alternada exteriormente a la función, como se comprobará en el código desarrollado por el alumno. Para la gestión conjunta, se le asigna al puerto SPI un semáforo. Habilitado y asignado el puerto SPI, se le asigna una velocidad de 8 MHz. Posteriormente, se le envía un comando al barómetro para que se reinicie, y posteriormente se obtiene la calibración del sensor aportada por el fabricante. Se inicia el proceso para obtener una medida, y si esa medida se tarda en obtener más de un segundo, se considera que el barómetro tiene problemas y se avisa al usuario sobre ello.  void _update (void): Lee los datos extraidos por el sensor, y la función es llamada, de media, cada 0.0075 segundos (no se permite que el proceso de medida dure más de 0.01 segundos). Para realizar esta tarea, se utiliza una máquina de estados de cinco estados: en el primero se lee la temperatura y en los cuatro posteriores se lee la presión; con cada llamada a la función el estado aumenta en una unidad. La razón es que la temperatura no necesita leerse tantas veces, ya que varía menos que la presión. Si no se llegan a tratar los datos, por la correspondiente función de tratamiento, antes de que exista un número determinado de datos acumulados (en el caso de la temperatura 32 valores, y en el de la presión 128) se perderán datos acumulados. En el momento que existe un dato acumulado, se avisa al sistema mediante un ‘flag’ de que hay medidas disponibles para tratar.  void read (void): Función que trata los datos obtenidos del sensor. Comprueba si la anterior función ha activado el ‘flag’ que indica que existen datos acumulados y, en caso afirmativo, captura esos datos y los reinicia para su uso en la función _update. Si los datos acumulados no son nulos (comprobación por seguridad), realiza una media de los datos acumulados, destinada a tratar posteriormente para obtener la presión y la temperatura en sus unidades correspondientes. Esta labor la lleva a cabo la función _calculate.  void _calculate (void): Función que calcula la presión en pascales y la temperatura en grados Celsius. Las operaciones no se detallarán en este escrito; para encontrar el detallado procedimiento de cálculo se puede acudir al ‘datasheet’ ofrecido por el fabricante [33]. Terminados los cálculos, la función almacena los resultados en dos variables: ‘temp’ y ‘press’, temperatura y presión, respectivamente.  float get_pressure (void): Esta función es usada frecuentemente por la librería AP_Baro.h/.cpp. Es, por denominarlo de forma sencilla, la interfaz entre el tratamiento a bajo nivel del sensor de presión 47 47 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale y el tratamiento del mismo a alto nivel. Esta función sólo realiza una tarea: devolver la variable ‘press’.  float get_temperature (void): Esta función, al igual que la anterior, es usada activamente por la librería superior. Realiza la misma función, pero relativa a la temperatura: devolver la variable ‘temp’. 5.1.1.3 AP_InertialSensor El conjunto de bibliotecas asociado a las acciones relacionadas con el acelerómetro y el giróscopo. Al igual que se expuso en los anteriores sensores, esta biblioteca contiene numerosos archivos destinados a definir el modo de actuación con respecto a cada uno de los modelos soportados por las plataformas que pueden cargar el firmware ArduCopter. En el caso de la APM 2.6, se dispone del popular acelerómetro/giróscopo MPU6000. A continuación se realizará un estudio de la librería general de este conjunto, AP_InertialSensor.h/.cpp, con descripciones de las funciones más relevantes. Funcionamiento general El funcionamiento aportado por la librería es bastante confuso. En una primera aproximación, podríamos definir tres niveles de tareas:  Tareas generales a todos los modelos de acelerómetros soportados, incluidas en AP_InertialSensor.h/.cpp.  Tareas específicas de cada modelo, definidas en sus respectivas bibliotecas (por ejemplo, AP_InertialSensor_MPU6000.h/.cpp).  Una librería que trabaja específicamente como nexo de unión entre el primer y segundo nivel, definiendo variables asociadas a cada sensor, llamada AP_InertialSensor_Backend.h/.cpp. Es la que proporciona directamente las medidas al primer nivel. Así, las tareas específicas de cada modelo de sensor suponen una clase subordinada a la definida en AP_InertialSensor_Backend, por lo que es esta biblioteca la que trata con el primer nivel. A continuación se realizará un estudio de la librería general de este conjunto, AP_InertialSensor.h/.cpp. AP_InertialSensor.h/.cpp: Funciones Las funciones que aparecen en AP_InertialSensor.h/.cpp son, entre otras:  void init (style, simple_rate): Inicia el sensor. Los parámetros que se le pasan son la manera de iniciar el sensor (primer parámetro) y el periodo de muestreo del sensor (segundo parámetro). El primer parámetro puede ser WARM_START, que supone la no calibración del acelerómetro y del giróscopo, o COLD_START, que supone la calibración del giróscopo por parte de la función. La función realiza, además, la asociación de una clase definida en AP_InertialSensor_Backend al sensor inicializado, que permite formalizar ese nexo de unión con el bajo nivel. También reinicia variables temporales útiles en las demás funciones, escalas a aplicar a las medidas obtenidas por el sensor y otras tareas.  void add_backend (AP_InertialSensor_Backend): Registra la definición de una clase puente entre alto y bajo nivel (parámetro), definida en AP_InertialSensor_Backend. Si se realiza con éxito, aumenta un contador de este tipo de clases en uno, existiendo un valor límite.  void detect_backend (void): Además de realizar la tarea especificada por add_backend, detecta qué modelo de sensor se está tratando y asocia la clase puente con la biblioteca de dicho modelo, cerrando la conexión entre bajo y alto nivel completamente.  void init_accel (void): Inicializa el acelerómetro calibrado. Para ello, realiza los siguientes pasos: 1. Se elimina el calibrado anterior, si lo hubiese. 2. Calibra el sensor con valores ‘offset’ muy altos, de tal manera que obliga al algoritmo tomar Firmware ArduCopter y creación de un firmware para la aeronave AirWhale 48 48 varias medidas para obtener un buen calibrado. 3. Comienza un bucle de calibración, donde se realizarán iteraciones hasta que se obtenga una calibración aceptable. Los siguientes pasos se realizarán dentro de una iteración. 4. Se toman medidas del sensor (función update) y se guarda como ‘offset_nuevo’, previo guardado del anterior como ‘offset_antiguo’. 5. A continuación, se tomarán cincuenta medidas para ir corrigiendo el ‘offset_nuevo’, realizando después de cada medición el siguiente cálculo: 𝑚𝑒𝑑𝑖𝑑𝑎𝑎𝑛𝑡𝑖𝑔𝑢𝑎=(𝑚𝑒𝑑𝑖𝑑𝑎𝑎𝑛𝑡𝑖𝑔𝑢𝑎·0.9)+(𝑚𝑒𝑑𝑖𝑑𝑎𝑛𝑢𝑒𝑣𝑎·0.2) El resultado de este cálculo será el nuevo ‘offset_nuevo’ candidato a adoptarse. 6. Se compara el resultado con el ‘offset_antiguo’, realizando las siguientes comprobaciones: 𝑐𝑎𝑚𝑏𝑖𝑜 𝑡𝑜𝑡𝑎𝑙=|offsetantiguo.x−offsetnuevo.x|+|offsetantiguo.y−offsetnuevo.y| +|offsetantiguo.z−offsetnuevo.z|<4 𝑜𝑓𝑓𝑠𝑒𝑡 𝑚𝑎𝑥𝑖𝑚𝑜=𝑚á𝑥(|offsetantiguo.x−offsetnuevo.x|,|offsetantiguo.x −offsetnuevo.x|,|offsetantiguo.x−offsetnuevo.x|)<250 Es decir, si el cambio total de medidas en cada eje entre una medida y otra es menor que 4, y el mayor cambio registrado entre medidas, sea cual sea su dirección cartesiana, es menor que 250, se da por buena la calibración y se guardan estos valores como ‘offset’. Si no se cumpliesen estas desigualdades, el algoritmo vuelve al paso 4.  void init_gyro (void): Inicializa, al igual que la anterior, el giróscopo. Realiza distintas operaciones, pero la idea puede considerarse semejante a la vista en la función init_accel. No se va a detallar.  void update (void): Se comunica con la clase asociada de tipo AP_InertialSensor_Backend para que realice las gestiones y cálculos concretos necesarios para obtener una medida de aceleración lineal y velocidad angular. Concretamente, la función esperará al siguiente instante de muestreo y ejecutará la función update, definida en AP_InertialSensor_Backend y, indirectamente, en cada una de las librerías asociadas a cada modelo de sensor. Una vez realizado esto, y otras tareas, avisa al sistema de que ya se ha iniciado el proceso para tomar las medidas.  Vector3f get_accel (void): Devuelve el vector de aceleraciones lineales actualizado, el cual está declarado globalmente y se modifica automáticamente con el uso de las demás funciones. Esta función SÓLO devuelve el vector.  Vector3f get_gyro (void): Devuelve el vector de velocidades angulares actualizado, el cual está declarado globalmente y se modifica automáticamente con el uso de las demás funciones. Esta función SÓLO devuelve el vector. AP_InertialSensor_Backend.h/.cpp: Descripción y funciones En esta librería se define la clase AP_InertialSensor_Backend, que añade la interfaz de variable y funciones necesarias entre alto y bajo nivel. Esta clase recibe como parámetro la clase definida en AP_InertialSensor, recibiendo sus variables para poder modificarlas. Las funciones definidas en esta librería más importantes son:  void rotate_and_offset_gyro (uint8_t, const Vector3f): Copia las medidas obtenidas por el giróscopo (segundo argumento) en la variable ‘_gyro’ del sensor correspondiente (primer argumento), definida en AP_InertialSensor; y la trata, orientando correctamente el vector de velocidades angulares y restándole los valores ‘offset’. Esta función es llamada en la librería AP_InertialSensor_MPU6000.h/.cpp.  void rotate_and_offset_accel (uint8_t, const Vector3f): Ídem a rotate_and_offset_gyro, 49 49 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale pero con las aceleraciones lineales. Además de los tratamientos mencionados, también aplica un escalado a las medidas. También se la llama en AP_InertialSensor_MPU6000.h/.cpp.  void set_accel_error_count (uint8_t, uint32_t): Copia el error obtenido en las medidas (segundo parámetro), del acelerómetro, en la variable correspondiente definida en AP_InertialSensor.  void set_gyro_error_count (uint8_t, uint32_t): Ídem a set_accel_error_count, pero con el error en las medidas del giróscopo. AP_InertialSensor_MPU6000.h/.cpp: Descripción y funciones Constituye la gestión del sensor a bajo nivel: definición de puertos, manejo de registros… y obtención directa de las medidas provenientes del MPU6000. Para ello, define una clase subordinada a la definida en la librería anterior: AP_InertialSensor_Backend. Así, cuando se llama desde alto nivel, por ejemplo, a la función update, indirectamente se llama a la función update definida en esta librería. Algunas de las funciones que se pueden encontrar en AP_InertialSensor_MPU6000.h/.cpp son:  bool init_sensor (void): Inicia el sensor a bajo nivel. Esto conlleva la inicialización del puerto SPI, la obtención de semáforos para la correcta gestión del mismo y la comprobación de que el sensor se ha iniciado correctamente, comprobando si se obtienen medidas por el puerto. También permite el inicio del muestreo continuo de medidas según el periodo de muestreo definido. Devuelve ‘true’ si la inicialización se ha realizado con éxito.  void poll_data (void): Esta función se ejecutará con cada periodo de muestreo, y es la que se encarga de tomar medidas por el sensor. Las medidas se guardan en variables tratadas, luego, por la función update.  bool update (void): Cuando es llamada, trata las variables obtenidas por la función poll_data para pasarlas a alto nivel, usando las funciones rotate_and_offset_gyro y rotate_and_offset_accel. 5.1.1.4 AP_AHRS Como se adelantaba anteriormente, esta biblioteca contiene el sistema que fusiona los datos de los tres sensores principales (acelerómetro, giróscopo y magnetómetro), filtrándolos para obtener la orientación en tiempo real de la aeronave (ángulos roll, pitch y yaw). Este filtrado está disponible en dos versiones:  Filtrado a través del Filtro de Kalman Extendido: Un filtrado eficaz capaz de proporcionar datos provenientes de los sensores ponderándolos con los datos que se deberían obtener mediante el modelo dinámico del sistema. El algoritmo de este filtro es bastante elaborado, y supone operaciones con matrices que, en caso de contar con multitud de variables de estado, implican una gran capacidad de cálculo. Su programación y uso está dirigida exclusivamente a placas con una capacidad de cómputo elevada, como PX4/PIXHAWK. Desafortunadamente, la plataforma APM 2.6 no dispone de capacidad suficiente como para poder implementar este filtro, por lo que no se ha hecho uso del mismo.  Filtrado DCM: Este tipo de filtrado, no muy conocido, es posible implementarlo en la plataforma APM 2.6. El funcionamiento básico se muestra en la Figura 5.1. Firmware ArduCopter y creación de un firmware para la aeronave AirWhale 56 56 - Clase asociada a AP_BoardConfig. - Clase asociada a AP_Motors. - Clases asociadas a las librerías de control. - Clase asociada a AP_BattMonitor. - Clase asociada a AP_InertialNav. - Clases relacionadas con otras funciones secundarias (AP_Relay, AP_Camera, AP_Sprayer…).  Definición de las tareas a realizar conjuntamente y a gestionar por el ejecutivo cíclico definido en AP_Scheduler. Para cada una de las dos velocidades que engloba cada una de las placas soportadas (400 Hz y 100 Hz, esta última la disponible en APM 2.6) existe una lista de acciones a controlar por el administrador de tareas. La lista es bastante larga, no se realizará una enumeración: cada una de las que aparecen disponen de distintos periodos y duraciones, y resulta interesante destacar los siguientes hechos: - A la velocidad más alta (100 Hz), se realiza la lectura del estimador DCM y del navegador inercial (InertialNav), se activan los controladores que requieren los datos de la IMU y los que ajustarán la orientación de la aeronave y se leen los datos del receptor de radio. - A la velocidad de 50 Hz, se lee la altitud y la velocidad en Z desde el navegador inercial, se comprueba si la aeronave está en tierra o ha aterrizado y hace comprobaciones relacionadas con el modo ‘Auto’. Además, si hay una cámara instalada, actualiza los controles de los servos para su movimiento y lleva a cabo labores de comunicación y guardado de datos. Por otro lado, actualiza la lectura del GPS - A la velocidad de 10 Hz, lee los datos sobre la batería y el magnetómetro, además del guardado de datos. También se actualiza los datos sobre la altitud a partir del barómetro y el sónar. - A la velocidad de 3.3 Hz, se realizan labores menos importantes o que requieran menos tiempo de refresco, como comprobar si se ha perdido comunicación con la estación en tierra o tareas relacionadas con AP_Sprayer o AP_Fence.  En el setup, se inicia la función init_ardupilot, la cual está definida en otro archivo .pde (system.pde), que se encarga de inicializar todo lo necesario para que Arducopter.pde pueda funcionar correctamente.  En el loop, el programa espera a la lectura del acelerómetro/giróscopo, que se recuerda que se realiza cada flanco de reloj de la APM 2.6. Una vez se ha realizado, se llama a la función fast_loop (que incluye lo definido anteriormente a 100 Hz) y se le comunica al administrador que puede dar paso a otra tarea (scheduler.tick) con tiempo disponible igual al momento en el que se leyó el acelerómetro, más el tiempo del reloj en microsegundos (100 Hz = 10000 µs) y menos el instante en el que se da paso a la tarea (tiempo de muestreo menos tiempo de ejecución en instante de muestreo). No se detallarán los archivos paralelos al Arducopter.pde, ya que no han aportado ninguna idea o concepto a aplicar al código desarrollado. Se recomienda, en cambio, revisarlos para conocer en profundidad el funcionamiento de Arducopter.pde y de los controladores. 5.2 Códigos desarrollados Una vez repasado de forma general el funcionamiento del código ArduCopter, se presenta el código desarrollado para volar la nave AirWhale. Este código se divide, principalmente, en tres archivos; cada uno de ellos incluye una funcionalidad diferente, según la fase de desarrollo del prototipo expuesto en el Capítulo 6. La razón es obvia: es necesaria la implementación del código en el ArduPilot Mega 2.6 y, además, la verificación del mismo con los motores conectados. Conforme el prototipo iba avanzando, el código lo hacía con él, aumentando sus posibilidades y tareas. Es necesario, pues, la definición de una línea temporal para marcar los hechos que rodean al código desarrollado, ya que el mismo depende fuertemente del contexto en el que se encuentra. Esta línea temporal, en 57 57 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale la que se puede repasar aproximadamente el contexto temporal de todo el trabajo presentado, aparece en la Figura 5.3. Como puede verificarse, existen dos archivos de código creados exclusivamente para el prototipo AirWhale, y se desea mejorar estos archivos y aumentar el número de los mismos en el curso que entra. Estos archivos son los siguientes:  Test_sensores_actuadores.pde: Código realizado para testear los sensores disponibles y a usar con el prototipo. Estos sensores son: acelerómetro, giróscopo, GPS y barómetro. También se incluye una opción de testeo de los motores conectados a la plataforma ArduPilot.  Sintonización.pde: Código utilizado para la sintonización y estabilización sin vuelo del prototipo. En él se definen por primera vez los controladores de los pares en X e Y, y el control de la altura Z. En la línea temporal se ve cómo el archivo de prueba de sensores y actuadores no pudo finalizarse hasta que no se dispusiese de los motores, ya que sin ellos no podía comprobarse si las funciones de control de actuadores eran eficaces. También, en este mismo sentido, el archivo de sintonización no pudo avanzar mucho en su desarrollo sin disponer de los actuadores, que suponen una de las componentes importantes en el proceso de sintonización in situ. ABRIL MAYO JUNIO JULIO AGOSTO SEPTIEMBRE Estudio Código ArduPilot Modelo dinámico Selección y compra de electrónica Construcción prototipo y vuelo Experimentación sensores y código Test_sensores_actuadores.pde Sintonizacion.pde Figura 5.3. Cronograma del trabajo desarrollado en este escrito, detallando los archivos de código creados Aportación monetaria a la asociación EsiTech Obtención de la electrónica Entrega TFG Firmware ArduCopter y creación de un firmware para la aeronave AirWhale 58 58 5.2.1 Código test_sensores_actuadores.pde Conforme se iba aprendiendo sobre el funcionamiento de las librerías del firmware ArduPilot, explicadas anteriormente, se fue desarrollando un código sencillo para el testeo y monitorización de las medidas ofrecidas por los sensores. Es un archivo que, pese a su sencillez, cobra gran importancia si se piensa en la trascendencia de los sensores en los códigos posteriores. El trabajo realizado con este código ahorra muchos quebraderos de cabeza a la hora de crear un código completamente desde cero, sin haber probado anteriormente si los sensores funcionan correctamente con las funciones adoptadas desde las bibliotecas. La creación de los siguientes códigos no se vio retrasada por problemas con los sensores. En una primera aproximación al código, su función se limita a, básicamente, obtener medidas de los sensores e imprimirlas por pantalla. En este sentido, se buscó familiarizarse con los procesos de inicialización y calibrado de los mismos, además de los modos de obtención de medidas. También se hace uso del estimador para obtener un estado de la orientación de la plataforma APM 2.6, y, con ello, la monitorización de las medidas de los sensores que se usan en el mismo (acelerómetro, giróscopo, magnetómetro y barómetro). Aparte de esto, se puede realizar un testeo de envío de valores PWM a los motores conectados a los pines de salida de la APM 2.6. Esta tarea fue de gran ayuda para cerciorarse de que los motores y los variadores se encontraban en un estado óptimo y, además, para asegurarse de que se conocía perfectamente el modo de controlar los motores mediante PWM. Funcionamiento general: Diagrama de flujo El funcionamiento general del programa se puede repasar en el diagrama de flujo de la Figura 5.4. Se pueden apreciar tres niveles generales:  Definición de las clases necesarias paa la gestión de sensores y actuadores.  Petición de acción deseada al usuario, que introducirá la opción por teclado.  Llamada a subrutinas que realizarán las medidas e impresión por pantalla de cada uno de los sensores. En el caso de los actuadores, se habla de una subrutina que envía PWM a cada motor. 59 59 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Figura 5.4. Diagrama de flujo del funcionamiento general de test_sensores_actuadores.pde Las clases definidas permiten acceder directamente a las funciones y variables definidas en cada una de las librerías asociadas a la misma. Estas son: - AP_Baro_MS5611 barometer - AP_Compass_HMC5843 compass - AP_InertialSensor ins - AP_AHRS ahrs - RC_Channel rc1, rc2, rc3 y rc4 - AP_MotorsQuad motors El programa solicita al usuario que pulse una letra en el teclado, según la tarea que desea realizar. Las tareas se enumeran como: - test_motores: Realiza una prueba del funcionamiento de los motores. - test_acel_gir: Realiza un testeo de la medición y cálculos relativos al acelerómetro/giróscopo. Firmware ArduCopter y creación de un firmware para la aeronave AirWhale 60 60 - test_barom: Realiza una prueba de las medidas aportadas por el barómetro. - test_AHRS: Aporta información sobre la orientación de la placa, fusionando datos del magnetómetro, acelerómetro/giróscopo y barómetro. Test_motores: funcionamiento y descripción detallada El objetivo principal de esta subrutina fue la de aprender exactamente cómo entregar el valor deseado de PWM a los motores. En un comienzo, la falta de conocimientos sobre el código ArduCopter y sobre la placa disponible, llevaban a la necesidad de probar si el procedimiento en mente concebido para controlar los motores era el correcto. Las dificultades encontradas sobre la gestión de los actuadores ya se han comentado en su respectiva librería. Se presenta el diagrama de flujo de la subrutina en cuestión (Figura 5.5). El programa lleva a cabo una solicitud de valores PWM a enviar a cada motor conectado a la plataforma APM 2.6. Para guardar los valores PWM tecleados por el usuario, se usa la función captura_datos, explicada más abajo. Una vez guardados los valores que se deben enviar a cada motor, el programa usa la función hal.rcout -> write (channel, PWM), la cual, como se explicó anteriormente en este escrito, envía por el pin de salida determinado por el primer parámetro el valor PWM determinado por el segundo parámetro. Para que esta función pueda realizar dicha tarea, debe haberse antes inicializado los motores con motors.init y motors.enable. Así, usa la función para enviar el valor PWM, espera dos segundos (delay) y envía un valor PWM nulo al motor para detenerlo. Repite el proceso para cada motor y, una vez acabada la secuencia, pregunta al usuario si desea repetir el test con distintos valores de PWM (instándole a que pulse ‘T’). En caso afirmativo, volverá a repetir el test, de lo contrario la subrutina acabará. Figura 5.5. Diagrama de flujo de la subrutina test_motores 61 61 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Test_acel_gir: funcionamiento y descripción detallada Una subrutina capaz de medir el acelerómetro y el giróscopo, sin calibrar y una vez calibrado, para observar los resultados por pantalla. Este hecho supone la comprobación exhaustiva de las medidas del acelerómetro/giróscopo según unos movimientos predeterminados; estos experimentos podrán repasarse más adelante. El programa, para empezar, inicializa el acelerómetro. Aquí cabe realizar una puntualización: el acelerómetro/giróscopo, al entrar en conflicto con el barómetro en el puerto SPI, debe inicializarse activando el pin CS asociado al mismo para una correcta inicialización. El pin CS se destina a dar paso a un dispositivo que comparta conexión SPI con otros dispositivos. Cada uno de los dispositivos dispone de un pin CS, de tal manera que, activado uno de ellos, la comunicación micro-dispositivo por SPI sólo se da para ese dispositivo. De igual manera se debe hacer con el barómetro, como se verá más adelante. Una vez inicializados, la recogida de medidas de uno y otro no supondrá ningún problema parecido. Esta tarea se realiza con las funciones hal.gpio - > pinMode (número pin, modo de transmisión del pin) y hal.gpio -> write (número pin, modo de transmisión del pin). Para el acelerómetro, el pin CS es el número 40. El diagrama de flujo se indica en la Figura 5.6. Figura 5.6. Diagrama de flujo de la subrutina test_acel_gir El funcionamiento es sencillo: se inicializa el sensor en modo WARM_START (como se pudo ver en la correspondiente librería, significa inicio sin calibración de ningún tipo) y a 100 Hz, con la función ins.init. Sin calibrar, se realizan doscientas medidas (tantas como indica el límite del contador impuesto) y se imprimirán por pantalla para su posterior análisis. Para ello, se usa la función ins.update, ins.get_accel e ins.get_gyro. Terminadas estas medidas, se reinicia el contador y se calibra el sensor, con ins.init_accel e ins.init_gyro. Con la calibración, se volverán a realizar doscientas medidas. Por último, se comunicará al usuario de que pulsando Firmware ArduCopter y creación de un firmware para la aeronave AirWhale 62 62 ‘A’ puede volver a realizar el test. En posteriores secciones se podrá comprobar los resultados obtenidos a partir de las medidas de esta subrutina ante movimiento predeterminados de la placa. Test_barom: funcionamiento y descripción detallada El funcionamiento de esta subrutina es semejante a la descrita anteriormente para el acelerómetro. Se inicializa el barómetro (barometer.init), teniendo en cuenta la multiconexión en el puerto SPI (en este caso, el pin CS para el barómetro es el 63), y se realizan doscientas medidas sin calibrar (barometer.read) y doscientas medidas habiendo calibrado el sensor. Todas ellas se imprimirán por pantalla para su posterior análisis. Esta vez, la letra para volver a realizar el test es ‘B’. El diagrama de flujo se muestra a continuación. Figura 5.7. Diagrama de flujo de la subrutina test_barom Test_AHRS: funcionamiento y descripción detallada El funcionamiento correcto del estimador pasa por inicializar los sensores GPS (GPS.init, no definida anteriormente debido a que sólo será utilizado para que el estimador pueda iniciarse, no se tendrán en cuenta sus medidas en este trabajo), acelerómetro y barómetro. Posteriormente, el programa será semejante a los anteriores, usando la función ahrs.init y accediendo a las variables globales definidas en la librería AP_AHRS que contienen los ángulos (ahrs.roll, ahrs.pitch y ahrs.yaw). 63 63 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Figura 5.8. Diagrama de flujo de la subrutina test_AHRS. 5.2.1.1 Experimentos realizados con el barómetro Los datos obtenidos mediante el programa descrito anteriormente sobre el barómetro son: - Presión, en Pa. - Altura, en metros. - Temperatura, en grados Celsius. Así, se van a realizar dos experimentos: - Primer experimento: Se eleverá lentamente la placa hasta aproximadamente un metro y diez centímetros de altura y volverá a dejarse sobre la altura cero. Se mostrarán los datos sin calibrar y los datos calibrados. Firmware ArduCopter y creación de un firmware para la aeronave AirWhale 64 64 Figura 5.9. Resultados experimento 1: presión sin calibrar Figura 5.10. Resultados experimento 1: temperatura sin calibrar La presión, al no estar calibrada inicialmente, debe sufrir un proceso estabilización en los primeros instantes de medida, ya que se parte desde una presión base ficticia nula. Posteriormente, se puede observar cómo la presión reduce su valor conforme se va elevando la plataforma APM 2.6. Cuando la plataforma va reduciendo su altura, la presión aumenta. Se ha intentado elevar y bajar la plataforma al mismo ritmo, hecho que se refleja en las medidas. Por su parte, la temperatura también debe partir desde valores más bajos, y aumenta considerablemente en los primeros instantes para, luego, y estabilizándose en un valor cercano a los 35,6 grados. La altitud no se ha representado, ya que sin una calibración previa, las funciones de cálculo de la altitud devuelven una altitud nula, por cuestiones de seguridad. Se debe calibrar el sensor para obtener medidas de la altura de la aeronave, obligatoriamente. 65 65 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Figura 5.11. Experimento 1: altitud calibrada. Figura 5.12. Experimento 1: presión calibrada Figura 5.13. Experimento 1: temperatura calibrada Firmware ArduCopter y creación de un firmware para la aeronave AirWhale 72 72 verticales para hacer que esas medidas tiendan al valor deseado. Cada uno de los motores influye de manera específica en cada una de las variables de estado y, por ende, en las variables que se desean controlar. La relación entre actuación individual de cada motor y las fuerzas y pares de actuación se exponen a continuación. El archivo sintonización.pde realiza la tarea descrita. Funcionamiento general: Diagrama de flujo El diagrama de flujo que representa el funcionamiento general del programa se muestra en la Figura 5.21. Principalmente está compuesto de los siguientes procedimientos:  Declaración de las variables globales necesarias y de las clases correspondientes a cada sensor, a los motores actuadores y al estimador ARHS. Además, se realiza una inicialización de cada uno de ellos de la siguiente manera: - El acelerómetro se inicializa en modo COLD_START (previa selección de su respectivo Chip Select), esto es, con la calibración implícita del giróscopo. Por ello, se añade en la siguiente línea de código la calibración sólo del acelerómetro. - Se inicia el barómetro seleccionando su CS y se calibra. - Inicialización del magnetómetro. - Inicialización de los motores y envío de un PWM nulo para la inicialización de los variadores.  Petición de cada uno de los parámetros de definen el control de cada variable de estado a controlar: - K, Ti y Td del controlador PID y referencia relacionados con el ángulo de cabeceo. - K, Ti y Td del controlador PID y referencia relacionados con el ángulo de alabeo. - K, Ti y Td del controlador PID y referencia relacionados con la altitud de la aeronave. Estos parámetros tendrán un valor distinto dependiendo del carácter de la actuación que proporcionen los controladores PID. Esto es muy importante a la hora de aplicar un valor de PWM a cada motor, ya que una sintonización y tratamiento posterior de las actuaciones no coordinadas entre sí puede llevar a la insuficiente fuerza de los motores para conseguir levantar la aeronave o el descontrol de la misma debido al valor desmesurado del PWM a pasar al motor. En el Capítulo 6 se indicarán los parámetros usados en los experimentos de estabilidad, el tratamiento de las actuaciones obtenidas y sus resultados, tanto a nivel teórico como a nivel de código. La obtención de los parámetros se realiza mediante la función captura_datos.  Inicio del bucle de control, donde se lleva a cabo: - La lectura del barómetro para obtener la altitud de la aeronave en tiempo real y la actualización del estimador para obtener los ángulos de orientación del vehículo en tiempo real. Estas medidas son necesarias para calcular el error, entre las medidas reales y las referencias introducidas, que será la entrada al PID. - El cálculo de las actuaciones con la aplicación de los controladores PID. - Tratamiento de las actuaciones según el carácter de las mismas. La medición del barómetro, como pudo apuntarse en la definición de su correspondiente librería, se realiza, máximo, a 0.01 segundos. Por otro lado, la estimación de los ángulos mediante el filtrado DCM puede realizarse sin dificultades a la misma velocidad. Sabiendo esto, el tiempo de muestreo de los controladores debe ser, mínimo, el valor del sensor de mayor periodo de lectura (en este caso, el barómetro). Es muy importante sincronizar estos tiempos, y, además, planificar el valor del tiempo de muestreo, por tres razones: 1. Es obvio que si el tiempo de muestreo de los controladores es menor que el periodo de medida de los sensores, existirán bucles de control con la misma medida del bucle anterior, ya que no habrá dado tiempo a obtener una medida nueva. Esto, evidentemente, no es deseable. 73 73 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Figura 5.25. Diagrama de flujo del funcionamiento general de sintonizacion.pde Firmware ArduCopter y creación de un firmware para la aeronave AirWhale 74 74 2. El tiempo de muestreo debe tener un valor lo suficientemente bajo como para aproximar el resultado de los controladores programados a los resultados extraídos de los controladores continuos basados en el modelo dinámico del sistema mostrado en el Capítulo 3. 3. El tiempo de muestreo debe ser mayor al tiempo de cálculo de Matlab, y el tiempo de envío y recepción de datos por puerto serie con el programa, en el caso del sistema ‘hardware in the loop’. El motivo es sencillo: si no es mayor, los datos aportados por el programa, que corresponden con las medidas de hipotéticos sensores, serían los mismos en varios bucles de control, y no es deseable. Como consecuencia de esto, hay que remarcar que el algoritmo programado correspondiente con la aplicación de los controladores PID depende estrechamente de la precisión del mismo con respecto al tiempo continuo. El algoritmo empleado para implementar el control PID se muestra en la ecuación (2): 𝑈𝑘=𝐾𝑝·[𝐸𝑘·(𝑇 𝑇𝑖·𝐼𝑘)·(𝑇𝑑 𝑇·(𝐸𝑘−𝐸𝑘−1))] Donde 𝐾𝑝, 𝑇𝑖 y 𝑇𝑑 son los parámetros característicos del controlador PID, 𝑈𝑘 es la actuación o salida del PID en el instante actual de muestreo, 𝐸𝑘 y 𝐸𝑘−1 son el error entre la medida y la referencia en el instante actual de muestreo y el anterior instante de muestreo, respectivamente; 𝑇 es el tiempo de muestreo del controlador e 𝐼𝑘 es el valor de la integración del error en cada instante de muestreo, y es igual a 𝐼𝑘=𝐼𝑘−1+𝐸𝑘, donde 𝐼𝑘−1 es el valor de la integración en el instante anterior de muestreo. El tratamiento de las actuaciones, salida de cada PID, es imprescindible, ya que representan los pares y fuerzas de control y no las actuaciones individuales de cada motor. Para obtener qué acción debe realizar cada motor para conseguir los pares y fuerzas deseadas, se usa el conjunto de ecuaciones mostradas en la sección 6.1 del presente trabajo, ya que están asociadas con la geometría del prototipo. La comprobación anti Wind-Up se basa en chequear si las actuaciones han llegado a saturar, momento en el que no es deseable que la componente integral del controlador PID siga integrando el error. Este hecho puede llevar al retardo en la respuesta instantes posteriores a un cambio en la referencia que necesite una actuación menor a la de saturación. Si la actuación satura, se activa un flag que determina que la integral del error del controlador asociado a esa actuación deja de actualizarse, manteniendo el valor de ese instante mientras la actuación siga saturando. En el momento en el que deja de saturar, el flag se desactiva y el efecto integral vuelve a actualizarse. Los experimentos de la estabilización de las variables de estado a controlar se muestran también en el Capítulo 6, donde se usará este código para obtener datos de validación. 5.2.3 Función captura_datos La función captura_datos es necesaria para la obtención de datos provenientes del usuario. La función que obtiene datos por el puerto serie, definida en el firmware ArduPilot, es hal.console->read. Esta sentencia recoge los datos obtenidos por teclado, pero en código ASCII. Esto es un problema, ya que el código ASCII no es un número fiel a lo introducido por el usuario. Por ello, se ha visto conveniente la creación de una función capaz de obtener estos datos y convertirlos en un número útil para las funciones de PWM y los controladores PID. El concepto es sencillo:  Se capturan los datos del teclado. En el caso de valores PWM (archivo test_sensores_actuadores) el número podrá ser un número entero entre 9999 y 0, mientras que en el caso de los controladores (archivo sintonizacion) podrá ser un número entre 99.99 y 00.00, siempre con dos decimales. El motivo se verá a continuación.  Se va obteniendo cada dato por teclado, de tal manera que se asociará el primer número leído como los millares (PWM) o decenas (controladores), el segundo número leído como centenas (PWM) o unidades (controladores), el tercer número como decenas o décimas y el último número como unidades o (2) 75 75 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale centésimas. Cualquier otro dato ASCII ajeno a un número se desecha.  Una vez obtenidos, se realizan las siguientes conversiones, teniendo en cuenta que los números entre 0 y 9 estarán, en ASCII, incluidos en el intervalo 48-57: - En el caso de PWM: 𝑃𝑊𝑀𝑓𝑖𝑛𝑎𝑙=(1000∗(𝑚𝑖𝑙𝑙𝑎𝑟𝑒𝑠−48))+(100∗(𝑐𝑒𝑛𝑡𝑒𝑛𝑎𝑠−48)) +(10∗(𝑑𝑒𝑐𝑒𝑛𝑎𝑠−48))+(1∗(𝑢𝑛𝑖𝑑𝑎𝑑𝑒𝑠−48)) - En el caso de los controladores: 𝑃𝑎𝑟𝑎𝑚𝑓𝑖𝑛𝑎𝑙=(10∗(𝑑𝑒𝑐𝑒𝑛𝑎𝑠−48))+(1∗(𝑢𝑛𝑖𝑑𝑎𝑑𝑒𝑠−48))+(0.1∗(𝑑é𝑐𝑖𝑚𝑎𝑠−48)) +(0.01∗(𝑐𝑒𝑛𝑡é𝑠𝑖𝑚𝑎𝑠−48)) 𝑃𝑊𝑀𝑓𝑖𝑛𝑎𝑙 y 𝑃𝑎𝑟𝑎𝑚𝑓𝑖𝑛𝑎𝑙 son los valores a usar por las funciones de PWM y los controladores, respectivamente. Construcción del prototipo AirWhale y experimentación 76 76 6 CONSTRUCCIÓN DEL PROTOTIPO AIRWHALE Y EXPERIMENTACIÓN a intención de construir y volar la aeronave AirWhale siempre ha sido el principal objetivo del equipo de desarrollo de la asociación EsiTech. Desde las primeras reuniones del equipo la idea de construir una versión final del AirWhale ha rondado la mente de todos los integrantes. Por desgracia, debido a la falta de tiempo y de recursos, se desechó la posibilidad de poder construir el vehículo. Sin embargo, el alumno José Luis Holgado Álvarez y yo vimos necesaria la construcción de una aeronave capaz de realizar movimientos parecidos al AirWhale y con una dinámica parecida, para la verificación del trabajo de José Luis y el mío, descrito en el Capítulo 5: el diseño teórico de controladores del estado dinámico de la aeronave AirWhale y la creación de un firmware que gestione adecuadamente los sensores y actuadores, además de otras funciones, a través de una plataforma autopiloto. Este trabajo tenía una evidente componente experimental en la verificación, y se contaba, en un principio, con la versión final de la aeronave AirWhale para pruebas de vuelo. Como ya se ha dicho, no ha sido así. Por ello, se decidió construir un prototipo con dinámica parecida y que pudiese operar con los dispositivos seleccionados para la versión final del AirWhale, de manera que los experimentos llevados a cabo con esta primera versión supusiesen una primera aproximación a lo que podría aportar el AirWhale en su versión final. Es decir: ensayar con una aeronave menos costosa, tanto en tiempo como en recursos de construcción, semejante en comportamiento dinámico y observar qué resultados (logros, problemas, posibilidades...) se pueden extraer de ello. En las siguientes secciones se repasarán todos los aspectos que rodean al prototipo: disposición geométrica, materiales y dispositivos electrónicos a usar, proceso de construcción, conexionado, estabilización estática y resultados. El diseño ha sido llevado a cabo por el alumno José Luis Holgado Álvarez. 6.1 Introducción: geometría y movimientos del prototipo La geometría del prototipo, debido a que se desea que guarde similitud dinámica con el AirWhale original, dispone de un globo de helio cilíndrico, con extremos semiesféricos, en el centro, rodeado de soportes para los motores verticales y horizontales, y demás electrónica. También dispone de una estructura de apoyo que evita que el globo contacte con el suelo. El diseño en CATIA del prototipo, realizado por el alumno José Luis Holgado Álvarez, se muestra en la Figura 6.1. Figura 6.1. Diseño conceptual del prototipo en CATIA [4] L 77 77 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Este diseño es orientativo; se verá en el proceso de construcción que varía con decisiones conjuntas tomadas durante el mismo. Como se puede observar, la disposición de los motores permiten realizar los mismos movimientos que la aeronave AirWhale: propulsión en X y Z, y giros en X, Y y Z. Los motores con eje vertical permiten controlar la propulsión en Z y el par en X, mientras que los motores con eje horizontal habilitan el control de la propulsión en X y el par en Z. El esquema inicial del prototipo, junto a sus movimientos, se muestra en la Figura 6.2. Para el control del par en el eje Y, la primera idea que se barajó es la de usar servomotores conectados físicamente a los motores traseros, de tal forma que, con un sencillo control en posición de ángulo, se pudiese controlar la dirección de la fuerza de dichos motores. Ello conlleva el diseño de un actuador capaz de realizar ese cambio de orientación, y el uso de dos servomotores para convertir en independientes el giro de cada motor. La razón de esto radica en el par de alabeo (par en X) residual que podría aparecer si la aeronave está actuando sobre el par en Z y, a la vez, sobre el par en Y. Controlar el par en Z supone la desigualdad de fuerzas proporcionadas por los motores traseros, y el control del par en Y supone el cambio de dirección de la fuerza ejercida por dichos motores. El resultado es equivalente a dos fuerzas de componente vertical distintas que generarían un par en X. Este concepto se explica en la Figura 6.3. En la figura, la componente 𝑓𝑥2𝑧 es mayor que 𝑓𝑥1𝑧, lo que supondría un desequilibrio de pares generados por estas dos fuerzas en el eje X y su consecuente resultante. Para realizar un control deseado del par en Y, las fuerzas en su componente vertical deben ser iguales. El ángulo α representa el ángulo de giro ejercido por los servos, y es el elemento que determina la magnitud de la componente vertical de ambas fuerzas. Para eliminar este efecto, el ángulo de giro en uno y otro motor deben ser distintos. Esto generaría, por otro lado, un 𝑓𝑥1 𝑓𝑥2 𝑓𝑧1 𝑓𝑧2 𝜽 𝜓 𝜙 𝑧0 𝑥0 𝑦0 𝑧 𝑥 𝑓𝑥1𝑥 𝑓𝑥2𝑥 𝑓𝑥2 𝑓𝑥2𝑧 𝑓𝑥1 𝑓𝑥1𝑧 α α 𝑧 𝑦 𝑥 Figura 6.2. Esquema básico e inicial del prototipo Figura 6.3. Esquema de la causa del par en X no deseable al girar los motores traseros el mismo ángulo Construcción del prototipo AirWhale y experimentación 78 78 desequilibrio añadido en las componentes horizontales de ambas fuerzas, por lo que el par que se desea generar en Z también se verá afectado, aumentando su valor. Por dicha complejidad, y por otros motivos que se verán en siguientes secciones, se ha optado por el uso de cuatro motores verticales en sustentación: dos para controlar el par en X (alabeo) y otros dos para controlar el par en Y (cabeceo), como se muestra en la Figura 6.4. El uso de servomotores en los actuadores traseros se pospondrá para trabajos posteriores. La relación, por tanto, entre pares y fuerzas de actuación, y las actuaciones individuales de cada motor, se muestra en la agrupación de ecuaciones (1): 𝑃𝑊𝑀 𝑚𝑜𝑡𝑜𝑟 1=𝐿1·𝑈𝑍−2·𝑈𝜃 4·𝐿1 𝑃𝑊𝑀 𝑚𝑜𝑡𝑜𝑟 2=𝐿2·𝑈𝑍+2·𝑈𝜙 4·𝐿2 𝑃𝑊𝑀 𝑚𝑜𝑡𝑜𝑟 3=𝐿1·𝑈𝑍+2·𝑈𝜃 4·𝐿1 𝑃𝑊𝑀 𝑚𝑜𝑡𝑜𝑟 4=𝐿2·𝑈𝑍−2·𝑈𝜙 4·𝐿2 Donde 𝐿1 y 𝐿2 son las distancias entre motores en el eje mayor y menor, respectivamente; y 𝑈𝑍, 𝑈𝜃 y 𝑈𝜙 son la fuerza vertical de control, el par sobre el eje Y de actuación y el par sobre el eje X de actuación, respectivamente. 𝑓𝑥1 𝑓𝑥2 𝑓𝑧1 𝑓𝑧2 𝜽 𝜓 𝜙 𝑧0 𝑥0 𝑦0 𝑓𝑧3 𝑓𝑧4 𝑧 𝑥 𝑦 (1) Figura 6.4. Esquema básico y final del prototipo 79 79 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale 6.2 Materiales de construcción y dispositivos electrónicos a usar El prototipo está contruido, en su totalidad, con madera de balsa administrado por el Departamento de Ingeniería Aeroespacial y Mecánica de Fluidos (aprovecho para dar las gracias al profesor Sergio Esteban Roncero por su colaboración y comprensión). Las piezas de madera usada cuentan con las siguientes características:  Madera de balsa sheets de 101.6 x 914.4 x 3.175 mm, de volumen 294.97 cm3, peso por unidad de 40.44 gramos y 0.1371 g/cm3 de densidad. Serán las piezas usadas para las superficies mayores de las alas y la plataforma donde se colocará la placa ArduPilot.  Madera de balsa sheets de 101.6 x 914.4 x 6.35 mm, de volumen 589.93 cm3, peso por unidad de 76.00 gramos y 0.1288 g/cm3 de densidad. Serán las piezas usadas para conformar los lados menores de la estructura principal y los listones que compondrán la estructura de apoyo del prototipo.  Madera de balsa strips de 9.525 x 914.4 x 3.175 mm, de volumen 27.65 cm3, peso por unidad de 5.85 gramos y 0.2117 g/cm3 de densidad. Serán, por tanto, los larguerillos usados para colocar en el lado menor, creando el perfil en I, y como superficie menor de las alas.  Madera de balsa sheets de 76.2 x 914.4 x 9.525 mm, de volumen 663.68 cm3, peso por unidad de 61.57 gramos y 0.0928 g/cm3 de densidad. Serán las piezas usadas para formas los lados mayores de la estructura principal. La electrónica a usar se enumera a continuación, encontrándose una explicación detallada de sus características en el Capítulo 4:  Plataforma ArduPilot, con módulo 3DR uBlox GPS.  Variadores EMAX 18 A Budget x 4.  Motores EMAX BL2210/30 x 4.  4 metros de cable AWG para el correcto conexionado entre los variadores y la batería.  Un batería Zippy 5000mAh.  Cable fino para la conexión entre variadores y plataforma autopiloto.  Dos conectores macho y uno hembra T-DEAN.  Un conector macho XT60.  Un adaptador T-DEAN – XT60. La razón por la que se ha escogido una batería en vez de dos es porque asociar una alimentación distinta a cada motor puede llegar a suponer distintos valores de empuje en los mismos. Hay que remarcar que, aunque el fabricante asegure que la batería tiene unas características predefinidas, no tiene por qué ser así: una y otra batería pueden disponer de parámetros distintos, y si esos parámetros (velocidad de descarga, carga…) son cruciales en vistas a la potencia proporcionada a los motores, los resultados en estabilización y vuelo pueden ser dispares. Por ello, se ha considerado el uso de una única alimentación, igualando las condiciones de alimentación para cada motor y eliminando ese posible problema. También se ha prescindido de la mayoría de elementos de conexionado, para facilitar el mismo. En este sentido, se han usado dos conectores macho-hembra T-DEAN para la conexión de alimentación del variador alimentador (se verá más adelante) y un conector macho para la conexión del cableado de alimentación con la batería. El adaptador T-DEAN – XT60 supone la unión entre el conector XT60 macho soldado a los cables de la batería y el conector T-DEAN del cableado de alimentación. Se verá con más detalle en la sección de conexionado. 6.3 Proceso de construcción El prototipo ha sido construido a lo largo de varios meses. El diseño del mismo, realizado en CATIA, dista bastante del vehículo elaborado finalmente, debido a decisiones que se han ido tomando a lo largo del proceso. Construcción del prototipo AirWhale y experimentación 80 80 Por otro lado, las dificultades que se han ido encontrando a lo largo de los meses han afectado directa o indirectamente a la forma final del prototipo. Todo ello se expone a continuación, en forma de pasos cronológicos:  En primer lugar, se midieron las longitudes de los listones de madera necesarios para los soportes principales del prototipo y las alas del mismo, donde se alojan dos de los motores. Las alas están compuestas de láminas de madera X (área superior e inferior) y de varillas de madera X. Los soportes principales están compuestos de dos listones de madera X (lado más largo) y de dos listones de madera X unidos por un larguerillo X, en forma de perfil en I.  Una vez medidas y cortadas, se ensamblan las maderas para crear los soportes principales y las alas. Las láminas de madera que conforman las alas se unen a los dos listones que formarán el lado superior del prototipo asociado a dicho ala, por la parte superior e inferior de los listones. En el caso de los lados menores, las superficies superior e inferior del perfil en I creado se unen a la superficie superior e inferior del lado mayor. Figura 6.5. Construcción del ala Figura 6.6. Medidas de los dos lados superiores con las alas colocadas 81 81 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Figura 6.7. Estructura principal Figura 6.8. Detalle de la unión entre lado menor y mayor de la estructura principal  Posteriormente, y ya obtenida la estructura principal del prototipo (la que confina el globo), se miden y se cortan las maderas necesarias para el soporte del prototipo en el suelo, que evitará que el globo entre en contacto con el mismo.  Por otro lado, se colocan los dos motores laterales en las alas. Estos se atornillan lo más cerca del extremo del ala posible, para crear el máximo par, y coincidentes con el eje menor del prototipo para no crear un par espúreo en Y.  Se unen los soportes del prototipo a la estructura principal que confina el globo, usando una pequeña Construcción del prototipo AirWhale y experimentación 88 88 peligrosos en la toma del USB). En la Figura 6.21 se puede observar cómo existen tres conexiones a través de dos cables y una sólo con tres cables (alimentación BEC). Figura 6.21. Conexionado placa-variadores Figura 6.22. Conexión T-DEAN para la alimentación del variador alimentador por BEC. Figura 6.23. Adaptador XT60 – T-DEAN conectado al cableado de alimentación 89 89 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale 6.5 Experimentos realizados Una vez se dispone del prototipo finalmente construido y con sus elementos electrónicos adecuadamente conectados, se exponen a continuación los distintos experimentos realizados con el mismo. Estos abarcan varios aspectos imprescindibles para el correcto vuelo de la aeronave, tales como la estabilización estática, identificación del sistema real para el uso de un controlador apropiado para el prototipo y estabilización dinámica. 6.5.1 Estabilización estática La estabilización de pesos del prototipo es un concepto muy importante si tenemos en cuenta la estabilización obligada de los ángulos de orientación de la aeronave en vuelo. Si estos ángulos tienen, de partida, un valor no corregido, se puede experimentar desviaciones indeseadas en el despegue y vuelo del vehículo. Para conseguir una estabilización estática, se ha fabricado un arcaico sistema para poder suspender la aeronave sobre dos puntos colocados en los extremos de sus dos ejes de simetría, como se puede observar en la figura. Figura 6.24. Experimentos realizados para la estabilización estática del vehículo sobre el eje Y En la imagen, el prototipo se suspende sobre su eje Y para observar qué desequilibrio de pesos existe inherente al mismo. Como puede comprobarse, el prototipo no se mantenía con un ángulo nulo sostenido por su eje Y, por lo que se optó por situar las baterías laterales un poco más hacia la izquierda. Cabe anotar que, aunque pueda parecer contradictorio el hecho de usar dos baterías teniendo en cuenta el conexionado anteriormente explicado, al introducir la electrónica usada ya se puntualizó el motivo por el que se rechazó la idea de usar dos baterías en vez de una. Colocadas las baterías correctamente, el vehículo se estabilizó. Construcción del prototipo AirWhale y experimentación 90 90 Figura 6.25. Estabilización sobre el eje Y conseguida Para la estabilización del ángulo relacionado con el eje X, se colocó el prototipo suspendido sobre su eje X, como se observa en la siguiente figura. Colocando un peso debajo del ala del lado levantado para igualar el exceso del otro lado, se consigue la estabilización. Figura 6.26. Experimento realizado para la estabilización estática sobre el eje X 91 91 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale Figura 6.27. Estabilización estática sobre eje X conseguida Figura 6.28. Peso colocado debajo del ala para la estabilización 6.5.2 Identificación del prototipo: sistema real La necesidad de una identificación en bucle abierto de la dinámica que ofrece el prototipo en su movimiento por la acción de pares es evidente. Se hizo patente al observar que la implementación de los controladores diseñados en el trabajo [4] era imposible, debido a que, al ser diseñados sobre un modelo teórico sin ruido, suponía un control muy rápido y brusco incapaz de controlar un sistema con ruido sin saturar constantemente. Es obvio que existen multitud de diferencias entre los modelos teóricos desarrollados y el modelo real representado por el prototipo, por lo que se necesitaba una representación del mismo en forma de función de transferencia para poder diseñar y aplicar controladores más conservadores y cercanos al sistema real. Por otro lado, el tratamiento de dichos controladores en términos de fuerza suponía una dificultad añadida a la hora de la implementación en el código desarrollado, ya que los valores enviados a los motores están en términos de PWM. En primera instancia, se barajó la idea de encontrar la característica estática entre el PWM que se mandaba a un motor y la fuerza que ejercía. Esta idea se simplificó a encontrar dos puntos de dicha característica y suponer una recta como relación entre PWM-fuerza. Este método no dio grandes resultados, por lo que identificar el sistema mediante una función de transferencia donde la entrada es PWM y la salida son los ángulos en cuestión parece más que adecuado. Para ello, se realizaron dos experimentos: la aplicación de un par escogido sobre el eje X del prototipo, estando este suspendido sobre dicho eje, y el proceso análogo con el eje Y. Para realizarlo, se modificó el código test_sensores_actuadores.pde, de tal manera que, en el test de motores, se pudiese dar un valor PWM a cada motor y mantener el valor de cada uno durante un tiempo determinado, mientras se imprimía por pantalla el valor de los ángulos roll y pitch. Construcción del prototipo AirWhale y experimentación 92 92  Identificación de la dinámica del prototipo par en X - ángulo roll: El par escogido de actuación será de 50 PWM, que, por las relaciones indicadas en (1), supone 36 y -36 PWM en los motores implicados en la aplicación de dicho par. Sumando los valores indicados a un valor inicial enviado a los motores implicados (1200 PWM) y leyendo en cada instante de muestreo (50 ms cada uno) el ángulo roll, se obtiene el siguiente resultado: Figura 6.29. Representación del valor del ángulo roll ante la aplicación en bucle abierto de un par en X Se puede observar su gran parecido con un sistema de segundo orden subamortiguado que tiende al valor de siete grados, aproximadamente. La añadida oscilación del final se debe a una ráfaga de aire.  Identificación de la dinámica del prototipo par en Y - ángulo pitch: El par escogido de actuación será de 60 PWM, que, por las relaciones indicadas en (1), supone 37.5 y -37.5 PWM en los motores implicados en la aplicación de dicho par. De forma análoga al anterior caso, pero con el ángulo pitch, se obtiene el siguiente resultado: Figura 6.30. Representación del valor del ángulo pitch ante la aplicación en bucle abierto de un par en Y El comportamiento de la respuesta se asemeja bastante a la obtenida en el caso del roll, pero con magnitudes 93 93 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale inferiores. La perturbación, en este caso, parece reducir un poco el valor en régimen permanente de la respuesta. El cálculo de las funciones de transferencia y de los controladores a aplicar se puede revisar en el trabajo [4]. 6.5.3 Estabilización dinámica de los ángulos roll y pitch El anterior experimento arroja, finalmente, unos controladores adecuados para el sistema real que representa el prototipo. Aplicando los parámetros de los controladores hallados en el código sintonizacion.pde (usado de forma íntegra, sin modificaciones), y dándoles un valor inicial de PWM a los motores de 1200, se observan los siguientes resultados, añadiendo, además, sendas perturbaciones al control:  Estabilización del ángulo roll: Figura 6.31. Evolución del ángulo roll y pitch durante la estabilización del ángulo roll y perturbaciones Figura 6.32. Evolución del error de los ángulos con respecto a la referencia durante la estabilización del ángulo roll Construcción del prototipo AirWhale y experimentación 94 94 La estabilización del ángulo roll es lenta, como puede comprobarse, encontrándose una sobreoscilación cercana al 50% y un tiempo de establecimiento superior a los 100 instantes de muestreo. También se puede observar que estos valores aumentan ante una perturbación como la que se aparece a los 200 y 450 instantes de muestreo. Por otro lado, no se observa que el valor del ángulo iguale la referencia nula que se le ha impuesto. Estas conclusiones no son positivas en vistas de un futuro vuelo. Figura 6.33. Par de actuación durante la estabilización del ángulo roll y perturbaciones Figura 6.34. PWM mandados a los motores laterales durante la estabilización del ángulo roll Los valores más altos de las actuaciones coinciden, obviamente, con los momentos en los que existe una gran perturbación. Conforme se va llegando a la referencia, las actuaciones disminuyen a valores aproximadamente de 15-16 PWM, en el caso del par de actuación, y 10 PWM para los motores individuales; intentarán mantener ese valor debido a la referencia distinta de cero. La saturación implementada a los actuadores son 60 y -60 PWM para el par de actuación, que se traduce en 43 y -43 PWM para cada motor. 95 95 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale  Estabilización del ángulo pitch: Figura 6.35. Evolución de los ángulos roll y pitch en la estabilización del ángulo pitch y perturbaciones Figura 6.36. Evolución del error de los ángulos con respecto a la referencia durante la estabilización del ángulo pitch El ángulo pitch, en este caso, presenta menos sobreoscilación, pero un mayor tiempo de establecimiento (alrededor de 250 instantes de muestreo). Tampoco llega a valer cero, quedándose en cerca de un grado de error con la referencia. Suponen resultados bastante mejorables. Construcción del prototipo AirWhale y experimentación 96 96 Figura 6.37. Par de actuación durante la estabilización del ángulo pitch y perturbaciones Figura 6.38. PWM mandados a los motores laterales durante la estabilización del ángulo pitch Las actuaciones siguen el mismo comportamiento mostrado en la anterior estabilización, siendo, en este caso, menor la velocidad con la que disminuye el valor PWM de las mismas (mayor sobreoscilación, mayor tiempo de establecimiento). Como puede comprobarse aquí también, se mantiene un valor residual debido al error con la referencia. La saturación del par en -50 – 50 PWM supone una saturación en la actuación individual de los motores de -33 – 33 PWM aproximadamente. En todos los experimentos se aprecia el efecto positivo del anti Wind-Up. La saturación de las actuaciones está presente en gran parte del experimento, no apreciándose apenas un retardo provocado por la integración del error en saturación. El sistema programado en el archivo sintonizacion.pde ofrece, pues, buenos resultados. Las estabilizaciones de los ángulos relacionadas con estos experimentos se pueden observar en los vídeos adjuntos a la memoria escrita. 97 97 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale 6.5.4 Comprobación de la dinámica ante un escalón dado en la fuerza de actuación vertical Debido a los problemas encontrados al intentar volar el prototipo con el control de ángulos activo, se vio necesaria la experimentación del comportamiento de la aeronave ante un escalón dado en la actuación en dirección Z. Este experimento es el más cercano al vuelo controlado que se ha realizado durante el trabajo presentado. Desgraciadamente, no se ha podido controlar la aeronave en un vuelo de forma decente: no se encontraba un control de ángulos efectivo, como se ha comentado en la anterior subsección. La aeronvae, al implementar la fuerza de actuación en Z, se descontrolaba bastante, haciendo imposible el vuelo normal de la misma. Dicho esto, se presentan los resultados de un experimento donde se aplica un escalón de 137.5 PWM a la actuación vertical (1750 PWM a cada motor individual, partiendo de un valor inicial de la actuación individual de 1200 PWM) y se controlan los ángulos, donde, como se puede ver en el vídeo adjunto, dos alumnos sujetan la aeronave para que la misma no se descontrole y se convierta en un elemento peligroso para las personas cercanas. Figura 6.39. Evolución de los ángulos roll (azul) y pitch (verde), y de la altitud (rojo) Figura 6.40. Evolución del error con respecto a la referencia de los ángulos roll y pitch ANEXO A 104 104 %% Ecuaciones linealizadas evaluando el punto de equilibrio ecl(1,1)=subs(xdl,{theta,phi,psi,U,V,W,mtx,mty,mtz,p,q,r,fx,fz},{0,0,0,5,0,0, 0,0,0,0,0,0,1.225*0.5*5^2*Cd*Shx,-he+g*m}); ecl(2,1)=subs(ydl,{theta,phi,psi,U,V,W,mtx,mty,mtz,p,q,r,fx,fz},{0,0,0,5,0,0, 0,0,0,0,0,0,1.225*0.5*5^2*Cd*Shx,-he+g*m}); ecl(3,1)=subs(zdl,{theta,phi,psi,U,V,W,mtx,mty,mtz,p,q,r,fx,fz},{0,0,0,5,0,0, 0,0,0,0,0,0,1.225*0.5*5^2*Cd*Shx,-he+g*m}); ecl(4,1)=subs(udl,{theta,phi,psi,U,V,W,mtx,mty,mtz,p,q,r,fx,fz},{0,0,0,5,0,0, 0,0,0,0,0,0,1.225*0.5*5^2*Cd*Shx,-he+g*m}); ecl(5,1)=subs(vdl,{theta,phi,psi,U,V,W,mtx,mty,mtz,p,q,r,fx,fz},{0,0,0,5,0,0, 0,0,0,0,0,0,1.225*0.5*5^2*Cd*Shx,-he+g*m}); ecl(6,1)=subs(wdl,{theta,phi,psi,U,V,W,mtx,mty,mtz,p,q,r,fx,fz},{0,0,0,5,0,0, 0,0,0,0,0,0,1.225*0.5*5^2*Cd*Shx,-he+g*m}); ecl(7,1)=subs(pdl,{theta,phi,psi,U,V,W,mtx,mty,mtz,p,q,r,fx,fz},{0,0,0,5,0,0, 0,0,0,0,0,0,1.225*0.5*5^2*Cd*Shx,-he+g*m}); ecl(8,1)=subs(qdl,{theta,phi,psi,U,V,W,mtx,mty,mtz,p,q,r,fx,fz},{0,0,0,5,0,0, 0,0,0,0,0,0,1.225*0.5*5^2*Cd*Shx,-he+g*m}); ecl(9,1)=subs(rdl,{theta,phi,psi,U,V,W,mtx,mty,mtz,p,q,r,fx,fz},{0,0,0,5,0,0, 0,0,0,0,0,0,1.225*0.5*5^2*Cd*Shx,-he+g*m}); ecl(10,1)=subs(phidl,{theta,phi,psi,U,V,W,mtx,mty,mtz,p,q,r,fx,fz},{0,0,0,5,0 ,0,0,0,0,0,0,0,1.225*0.5*5^2*Cd*Shx,-he+g*m}); ecl(11,1)=subs(thetadl,{theta,phi,psi,U,V,W,mtx,mty,mtz,p,q,r,fx,fz},{0,0,0,5 ,0,0,0,0,0,0,0,0,1.225*0.5*5^2*Cd*Shx,-he+g*m}); ecl(12,1)=subs(psidl,{theta,phi,psi,U,V,W,mtx,mty,mtz,p,q,r,fx,fz},{0,0,0,5,0 ,0,0,0,0,0,0,0,1.225*0.5*5^2*Cd*Shx,-he+g*m}); ecl 105 105 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale ANEXO B La siguiente tabla muestra los productos comprados junto a su enlace de compra: Tabla 0.A. Enlaces web de compras de los productos definidos en el Capítulo 4 Nombre Enlace Motor Brushless EMAX BL2210/30 http://www.rctecnic.com/es/motores-brushless- para-aviones-rc/557-motor-brushless-outrunner- emax-bl221030-1300kv Regulador Brushless EMAX 18A Budget http://www.rctecnic.com/es/reguladores/2271- regulador-brushless-emax-18a-budget-esc-2a- bec-2-3s Bateria Zippy 5000mAh 3S http://www.hobbyjess.es/es/product/bateriazippy-5000mah-3s111v-20-30c Adaptador XT60 macho a t-dean hembra http://www.hobbyjess.es/es/product/adaptadorde-xt60-macho-a-tdean-hembra-1 Cable de silicona 16AWG (1 metro) http://www.rctecnic.com/es/cable-de- silicona/801-cable-de-silicona-diametro-29mm- 16awg-1-metro Conector tdean macho hembra http://www.hobbyjess.es/es/product/conectortipo-dean-t-rojo-macho-hembra Conector XT60 hembra macho http://www.hobbyjess.es/es/product/conectorxt60-hembra-macho Cargador IMAX B6AC http://www.hobbyjess.es/es/product/cargadorbalanceador-hk-50w-5a-12v-digital Servo TowerPro SG92r 9G http://www.hobbyjess.es/es/product/servotowerpro-sg90-9g EMAX Program Card http://www.rctecnic.com/es/reguladores/1910- emax-program-card-para-reguladores-brushless- emax Placa de distribución de corriente http://www.rctecnic.com/es/electronicaaccesorios-y-fpv-para-multicopteros/3845-placa- de-distribucion-de-corriente-para-cuadricoptero- con-conector-t-dean Hélice de Fibra de Carbono 9x4,7" (CCW) http://www.rctecnic.com/es/helices-de-fibra-de- carbono/3582-helice-de-fibra-de-carbono-9x47- ccw-sentido-normal-dji Hélice de Fibra de Carbono 9x4,7" (CW) http://www.rctecnic.com/es/helices-de-fibra-de- carbono/3581-helice-de-fibra-de-carbono-9x47- cw-sentido-contrarrotatorio-dji ANEXO B 106 106 Las facturas de las compras realizadas se muestran a continuación. Se hicieron varias compras a lo largo de los meses, por lo que los gastos de envío aparecen varias veces en ellas. No se asocia ninguna identificación a las mismas. 107 107 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale ANEXO B 108 108 109 109 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale ANEXO B 110 110 111 111 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale ANEXO C 112 112 ANEXO C Se presentan los códigos incluidos en test_sensores_actuadores.pde y sintonizacion.pde. Código 1. Código incluido en test_sensores_actuadores.pde /* CODIGO ARDUPILOT AIRWHALE - TEST SENSORES Y ACTUADORES - ALUMNO: ALEJANDRO ROMERO GALAN Este codigo ha sido desarrollado integramente por el alumno Alejandro Romero Galan como parte de su Trabajo Fin de Grado en Ingenieria de las Tecnologias Industriales de la Universidad de Sevilla (ETSI). */ #include <AP_Common.h> #include <stdarg.h> #include <AP_Progmem.h> #include <AP_Param.h> #include <AP_HAL.h> #include <AP_HAL_AVR.h> #include <AP_HAL_PX4.h> 113 113 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale #include <AP_HAL_Linux.h> #include <AP_HAL_Empty.h> #include <AP_Math.h> #include <RC_Channel.h> #include <AP_Motors.h> #include <AP_Curve.h> #include <AP_Notify.h> #include <AP_GPS.h> #include <AP_Mission.h> #include <DataFlash.h> #include <AP_InertialSensor.h> #include <AP_ADC.h> #include <GCS_MAVLink.h> #include <AP_Baro.h> #include <Filter.h> #include <AP_AHRS.h> #include <AP_Compass.h> #include <AP_Declination.h> #include <AP_Airspeed.h> #include <AP_Vehicle.h> #include <AP_ADC_AnalogSource.h> #include <AP_Mission.h> #include <StorageManager.h> #include <AP_Terrain.h> #include <AP_NavEKF.h> #include <AP_ADC.h> #include <AP_Rally.h> ////////////////////////////////////////////////////////////////////// ///// AP_Hal 'instance' ////////////////////////////////////////////////////////////////////// const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; ////////////////////////////////////////////////////////////////////// ///// Barómetro ////////////////////////////////////////////////////////////////////// static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi); ////////////////////////////////////////////////////////////////////// ///// Magnetómetro ////////////////////////////////////////////////////////////////////// static AP_Compass_HMC5843 compass; ////////////////////////////////////////////////////////////////////// ///// Acelerómetro ////////////////////////////////////////////////////////////////////// AP_InertialSensor ins; ////////////////////////////////////////////////////////////////////// ///// Motores ////////////////////////////////////////////////////////////////////// RC_Channel rc1(0), rc2(1), rc3(2), rc4(3); AP_MotorsQuad motors(rc1, rc2, rc3, rc4); ANEXO C 120 120 hal.console->printf("\n"); hal.console->printf("\n"); hal.console->println("Fin del test, pulsa E si quieres volver a realizar un test\n"); while( hal.console->available() ) { hal.console->read(); hal.scheduler->delay(20); } while( !hal.console->available() ) { hal.scheduler->delay(20); } hal.console->printf("\n"); hal.console->printf("\n"); opcion=hal.console->read(); // Si se pulsa 'e', se inicia el test de nuevo if(opcion == 'e' || opcion == 'E'){ } else{ fin = 1; } hal.scheduler->delay(2000); } hal.console->println("Fin del test del estimador DCM"); } float captura_datos(void){ int16_t valor_consola; int16_t pwm_mil; int16_t pwm_cien; int16_t pwm_diez; int16_t pwm_uno; float pwm_final; int contador = 0; int fin = 0; // Se captura los datos del puerto serie hal.scheduler->delay(200); while(fin == 0){ while( !hal.console->available() ) { hal.scheduler->delay(20); } valor_consola = hal.console->read(); if(valor_consola == 13 || (valor_consola<48 && valor_consola>57)){ hal.console->println("Dato a desechar"); if(contador > 3) { fin=1; hal.console->println("Fin lectura"); } } else{ if(contador == 0){ pwm_mil = valor_consola; hal.console->println("Leido dato decena"); 121 121 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale } if(contador == 1){ pwm_cien = valor_consola; hal.console->println("Leido dato unidad"); } if(contador == 2){ pwm_diez = valor_consola; hal.console->println("Leido dato decima"); } if(contador == 3){ pwm_uno=valor_consola; hal.console->println("Leido dato centesima"); } contador++; } } // Se halla el valor para el cálculo en PID pwm_final=(1000*(pwm_mil-48))+(100*(pwm_cien-48))+(10*(pwm_diez- 48))+(pwm_uno-48); return pwm_final; } AP_HAL_MAIN(); Código 2. Código incluido en sintonizacion.pde /* CODIGO ARDUPILOT AIRWHALE - SINTONIZACION - ALUMNO: ALEJANDRO ROMERO GALAN Este codigo ha sido desarrollado integramente por el alumno Alejandro Romero Galan como parte de su Trabajo Fin de Grado en Ingenieria de las Tecnologias Industriales de la Universidad de Sevilla (ETSI). */ #include <AP_Common.h> #include <stdarg.h> #include <AP_Progmem.h> #include <AP_Param.h> #include <AP_HAL.h> #include <AP_HAL_AVR.h> ANEXO C 122 122 #include <AP_HAL_PX4.h> #include <AP_HAL_Linux.h> #include <AP_HAL_Empty.h> #include <AP_Math.h> #include <RC_Channel.h> #include <AP_Motors.h> #include <AP_Curve.h> #include <AP_Notify.h> #include <AP_GPS.h> #include <AP_Mission.h> #include <DataFlash.h> #include <AP_InertialSensor.h> #include <AP_ADC.h> #include <GCS_MAVLink.h> #include <AP_Baro.h> #include <Filter.h> #include <AP_AHRS.h> #include <AP_Compass.h> #include <AP_Declination.h> #include <AP_Airspeed.h> #include <AP_Vehicle.h> #include <AP_ADC_AnalogSource.h> #include <AP_Mission.h> #include <StorageManager.h> #include <AP_Terrain.h> #include <AP_NavEKF.h> #include <AP_ADC.h> #include <AP_Rally.h> #include <AP_Scheduler.h> #include <AP_Compass_HMC5843.h> #include <AP_InertialNav.h> #include <AP_Buffer.h> ////////////////////////////////////////////////////////////////////// ///// AP_Hal 'instance' ////////////////////////////////////////////////////////////////////// const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; ////////////////////////////////////////////////////////////////////// ///// Barómetro 123 123 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale ////////////////////////////////////////////////////////////////////// static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi); ////////////////////////////////////////////////////////////////////// ///// Magnetómetro ////////////////////////////////////////////////////////////////////// static AP_Compass_HMC5843 compass; ////////////////////////////////////////////////////////////////////// ///// Acelerómetro ////////////////////////////////////////////////////////////////////// AP_InertialSensor ins; ////////////////////////////////////////////////////////////////////// ///// Motores ////////////////////////////////////////////////////////////////////// RC_Channel rc1(0), rc2(1), rc3(2), rc4(3); AP_MotorsQuad motors(rc1, rc2, rc3, rc4); ////////////////////////////////////////////////////////////////////// ///// GPS ////////////////////////////////////////////////////////////////////// static AP_GPS gps; ////////////////////////////////////////////////////////////////////// ///// Estimador DCM ////////////////////////////////////////////////////////////////////// AP_AHRS_DCM ahrs(ins, barometer, gps); ////////////////////////////////////////////////////////////////////// ///// Variables globales ////////////////////////////////////////////////////////////////////// float Ref_roll = 0; // Referencia roll float Ref_pitch = 0; // Referencia pitch ANEXO C 124 124 float Ref_z; // Referencia Z float Kp_roll, Ti_roll, Td_roll; // Parámetros del PID del controlador Roll float Kp_pitch, Ti_pitch, Td_pitch; // Parámetros del PID del controlador Pitch float Kp_z, Ti_z, Td_z; // Parámetros del PID del controlador altura en Z int T_roll = 20; // Tiempo de muestreo control roll int T_pitch = 20; // Tiempo de muestreo control pitch int T_z = 20; // Tiempo de muestreo control Z float Uk_roll; // Variable de actuación (salida del PID control roll) float Uk_pitch; // Variable de actuación (salida del PID control pitch) float Uk_z; // Variable de actuación (salida del PID control Z) float Ik_roll, Ik1_roll; // Variables para el término integral float Ik_pitch, Ik1_pitch; // Variables para el término integral float Ik_z, Ik1_z; // Variables para el término integral float Ek_roll, Ek1_roll; // Variables para el error float Ek_pitch, Ek1_pitch; // Variables para el error float Ek_z, Ek1_z; // Variables para el error ///////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////// SETUP ///////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////// void setup() { // Inicio acelerometro hal.gpio->pinMode(40, HAL_GPIO_OUTPUT); hal.gpio->write(40, 1); ins.init(AP_InertialSensor::COLD_START, AP_InertialSensor::RATE_100HZ); // Inicio barometro hal.gpio->pinMode(63, HAL_GPIO_OUTPUT); hal.gpio->write(63, 1); barometer.init(); barometer.calibrate(); // Inicio magnetómetro 125 125 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale compass.init(); // Inicio motores motors.Init(); motors.enable(); hal.rcout->write(0,0); hal.rcout->write(1,0); hal.rcout->write(2,0); hal.rcout->write(3,0); // Inicio estimador ahrs.init(); hal.scheduler->delay(1000); } ///////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////// Bucle principal ///////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////// void loop() { // Definiciones float altura; float PWM_motor1 = 0; float PWM_motor2 = 0; float PWM_motor3 = 0; float PWM_motor4 = 0; float L1 = 0.8; float L2 = 0.7; int fin = 0; int anti_roll = 0; int anti_pitch = 0; int anti_z = 0; int16_t tecla; float Fz=0; ANEXO C 126 126 ///////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////// Sintonización ///////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////// hal.console->println("Sintonizacion de controladores. Valores pedidos: Kp, Ti, Td y referencia\n"); hal.console->println("Sintonizacion controlador PID para el roll\n"); /////////////// /// Kp_roll /// /////////////// // Se pide un valor para los parámetros de los controladores hal.console->println("Introduce un valor de Kp_roll\n"); Kp_roll = captura_datos(); /////////////// /// Ti_roll /// /////////////// // Se pide un valor para los parámetros de los controladores hal.console->println("Introduce un valor de Ti_roll\n"); Ti_roll = captura_datos(); /////////////// /// Td_roll /// /////////////// // Se pide un valor para los parámetros de los controladores hal.console->println("Introduce un valor de Td_roll\n"); Td_roll = captura_datos(); //////////////// /// Ref_roll /// //////////////// 127 127 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale // Se pide un valor para los parámetros de los controladores hal.console->println("Introduce un valor de Ref_roll (en grados)\n"); Ref_roll = captura_datos(); //////////////// /// Kp_pitch /// //////////////// hal.console->println("Sintonizacion controlador PID para el pitch\n"); // Se pide un valor para los parámetros de los controladores hal.console->println("Introduce un valor de Kp\n"); Kp_pitch = captura_datos(); //////////////// /// Ti_pitch /// //////////////// // Se pide un valor para los parámetros de los controladores hal.console->println("Introduce un valor de Ti\n"); Ti_pitch = captura_datos(); //////////////// /// Td_pitch /// //////////////// // Se pide un valor para los parámetros de los controladores hal.console->println("Introduce un valor de Td\n"); Td_pitch = captura_datos(); ///////////////// /// Ref_pitch /// ///////////////// // Se pide un valor para los parámetros de los controladores hal.console->println("Introduce un valor de Ref_pitch (en grados)\n"); Ref_pitch = captura_datos(); //////////// /// Kp_z /// //////////// ANEXO C 128 128 hal.console->println("Sintonizacion controlador PID para Z\n"); // Se pide un valor para los parámetros de los controladores hal.console->println("Introduce un valor de Kp\n"); Kp_z = captura_datos(); //////////// /// Ti_z /// //////////// // Se pide un valor para los parámetros de los controladores hal.console->println("Introduce un valor de Ti\n"); Ti_z = captura_datos(); //////////// /// Td_z /// //////////// // Se pide un valor para los parámetros de los controladores hal.console->println("Introduce un valor de Td\n"); Td_z = captura_datos(); ///////////// /// Ref_z /// ///////////// // Se pide un valor para los parámetros de los controladores hal.console->println("Introduce un valor de Ref_z (en metros)\n"); Ref_z = captura_datos(); //// OPCION DE PEDIR ESCALÓN PARA FZ //// // hal.console->println("Introduce un valor de Fz (en PWM)\n"); // Fz = captura_datos(); // Fz = 100*Fz; hal.console->println("Fin de sintonizacion. Iniciando controladores\n"); 129 129 Revisión y Modificación del Firmware de Libre Acceso ArduCopter para su uso en el Proyecto AirWhale ///////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////// Controladores ///////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////// hal.console->printf("Roll: \t Pitch: \t Altura: \t Error Roll: \t Error Pitch: \t Error Z: \t Actuacion Roll: \t Actuacion Pitch: \t Actuacion Z: \t Fuerza motor 1: \t Fuerza motor 2: \t Fuerza motor 3: \t Fuerza motor 4: \n"); hal.rcout->write(0,0); hal.rcout->write(1,0); hal.rcout->write(2,0); hal.rcout->write(3,0); //// INICIO BUCLE DE CONTROL //// while(fin == 0){ ///////////////// //// Lectura sensor ///////////////// ahrs.update(); // Se activa el estimador, obteniendo los ángulos de Euler barometer.read(); // Se lee barómetro altura = barometer.get_altitude(); // Se guarda la altitud ///////////////// //// Controlador Roll ///////////////// Ek_roll = Ref_roll - ToDeg(ahrs.roll); // Se calcula el error if(anti_roll == 1){ // Comprobación de anti Wind-Up Ik_roll = Ik1_roll; }else{ Ik_roll = Ik1_roll + Ek_roll; // Se actualiza integrador } Uk_roll = (Kp_roll * (Ek_roll + (((T_roll/1000)/Ti_roll)*Ik_roll) +