Dise No y Construcci On de Drone para Tareas de Inspecci On en Mantenimiento

Facultad de Ingenierı́a
Escuela de Ingenierı́a Civil Mecatrónica
Diseño y construcción de drone para tareas

de inspección en mantenimiento
Profesor Guı́a:
Roberto Ramirez
Gonzalo Rojas Garcı́a

Curico-Chile
2021
CONSTANCIA
La Dirección del Sistema de Bibliotecas a través de su encargado Biblioteca Campus Curicó certifica
que el autor del siguiente trabajo de titulación ha firmado su autorización para la reproducción en
forma total o parcial e ilimitada del mismo.
Curicó, 2022
Vicerrectoría Académica | Dirección de Bibliotecas

Diseño y construcción de drone para tareas
de inspección en mantenimiento
Junio, 2021
Diseño y construcción de drone para tareas de inspección en mantenimiento
Resumen
En la presente memoria se desarrolla y expone los puntos más relevantes sobre el
proyecto de diseño y construcción de drone para tareas de inspección en mantenimiento,
tomando en cuenta desde una etapa temprana considerando cálculos y simulaciones
teóricas hasta el desarrollo correspondiente a la construcción de un prototipo orientado
al control sobre la variable de altura.
Se propone la integración de un control PID, en conjunto con el desarrollo de un
sistema basado en la metodologı́a ROS a través de comunicación inalámbrica con un
computador maestro para realizar la estimación de posición del prototipo.
Del trabajo realizado y mediante la experimentación con el prototipo construido se
obtuvo resultados que permiten identificar puntos de mejora y sus posibles alternativas
para un desarrollo futuro.
Gonzalo Rojas Garcı́a i

Dedicado a
todos aquellos que hicieron esta memoria posible
Gonzalo Rojas Garcı́a ii

Agradecimientos
Primero que todo agradezco a mis padres Carlos y Raquel por todo su apoyo en mi formación
tanto profesional como persona, a mi polola Belén por motivarme y apoyarme durante estos
años, además de ser un pilar fundamental en cada uno de estos logros. Agradezco a mi
hermano Francisco, por creer en mis capacidades y brindarme su apoyo en todo momento,
a mi abuela Marı́a Eliana y a mi familia en general por apoyarme en lo que fuese necesario
para el logro de esta meta. Por ultimo y no menos importante agradezco a mi profesor guı́a
Roberto Ramı́rez y al profesor Daniel Diaz por su excelente disposición y constante apoyo a
mi formación como Ingeniero, por sobre todo en el desarrollo de la presente memoria.
Gonzalo Rojas Garcı́a iii

Índice
Acrónimos X
1 Introducción 1
1.1 Introducción general . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Estado del arte . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2.1 Gestión de activos . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2.2 Vehı́culos aéreos no tripulados (UAV) . . . . . . . . . . . . . . . . . . 3
1.2.3 Estrategias de control . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.2.4 Discusión estado del arte . . . . . . . . . . . . . . . . . . . . . . . . . 11
1.2.5 Objetivo general . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.2.6 Objetivos especı́ficos . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.3 Alcances y limitaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.3.1 Alcances . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.3.2 Limitaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.4 Metodologı́a . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.4.1 Revisión bibliográfica . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.4.2 Modelamiento y simulación matemática . . . . . . . . . . . . . . . . . 14
1.4.3 Diseño y selección de componentes . . . . . . . . . . . . . . . . . . . 14
1.4.4 Resultados experimentales . . . . . . . . . . . . . . . . . . . . . . . . 14
1.5 Temario . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2 Desarrollo teórico 16
2.1 Modelamiento matemático . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.1.1 Sistemas de referencia . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.1.2 Comportamiento del UAV . . . . . . . . . . . . . . . . . . . . . . . . 18
2.1.3 Algoritmo de mezcla de motores . . . . . . . . . . . . . . . . . . . . . 21
2.2 Simulación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.2.1 Parámetros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
2.2.2 Controlador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
2.2.3 Simulación con control sobre Roll, Pitch, Yaw y Thrust . . . . . . . . 23
2.2.4 Simulación con control sobre posición X, Y y Z . . . . . . . . . . . . 27
3 Etapa de diseño 30
3.1 Componentes necesarios para construccion de un UAV . . . . . . . . . . . . 30
3.1.1 Motores brushless . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.1.2 ESC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.1.3 Controlador de vuelo . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.1.4 Baterı́a LiPo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.1.5 Hélices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.1.6 PDB . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.1.7 Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.1.8 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.1.9 Barómetro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.1.10 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
iv
3.1.11 Sensor ultrasónico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

3.1.12 Cámara . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
3.2 Diseño y selección de componentes . . . . . . . . . . . . . . . . . . . . . . . 34
4 Etapa de construcción 43
4.1 Ensamble mecánico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
4.2 Conexionado electrónico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
4.3 Programación y control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
4.3.1 Verificación de módulos . . . . . . . . . . . . . . . . . . . . . . . . . . 55
4.3.2 Transcripción de bibliotecas . . . . . . . . . . . . . . . . . . . . . . . 61
4.3.3 Generación de PWM . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
4.3.4 ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
4.3.5 Algoritmo de control . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
4.3.6 Comunicación entre tarjetas . . . . . . . . . . . . . . . . . . . . . . . 78
5 Resultados 80
5.1 Revisión del funcionamiento del sistema integrado . . . . . . . . . . . . . . . 80
5.2 Primera prueba de vuelo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
5.3 Control con compensación manual . . . . . . . . . . . . . . . . . . . . . . . . 85
6 Conclusiones y trabajo futuro 90

6.1 Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
6.2 Trabajo futuro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
Referencias 93
Anexos 96
A Control sobre Roll, Pitch, Yaw y Thrust 96
B Control sobre posición en ejes X, Y y Z 97
C Plano de placa de montaje para Cora Z7 98
D Código ejemplo para uso de barómetro 99
E Código ejemplo para uso de la IMU 100
F Código ejemplo para uso de sensor GPS 103
G Biblioteca .h para barómetro 105
H Código .cc para barómetro 109
I Biblioteca .h para IMU 120
J Código .cc para IMU 128
K Código AXI-PWM.v 167
Gonzalo Rojas Garcı́a v

L Código PWM.v 178
M Código de ejemplo para PWM en Xilinx SDK 180
N Diagrama de relaciones ROS del proyecto 182
Ñ Código con monitoreo por puerto serial para controlador de vuelo 183
O Código en lazo abierto para cálculo de compensaciones por motor. 196
Gonzalo Rojas Garcı́a vi

Índice de figuras
1.1 Diagrama de principales tipos de UAV’s . . . . . . . . . . . . . . . . . . 4
1.2 Diagrama de principales tipos de UAV’s de hélice rotatoria . . . . . . . . 5
1.3 Tipos de control más usados en UAV’s . . . . . . . . . . . . . . . . . . . 7
1.4 Diagrama de bloques para controlador fuzzy . . . . . . . . . . . . . . . . 9
1.5 Ejemplo de red neuronal . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.1 Sistema coordenado inercial del drone. . . . . . . . . . . . . . . . . . . . 17
2.2 Sistema coordenado del cuerpo del drone. . . . . . . . . . . . . . . . . . . 17
2.3 Diagrama de bloques control PD. . . . . . . . . . . . . . . . . . . . . . . 23
2.4 Diagrama de bloques control PID. . . . . . . . . . . . . . . . . . . . . . . 23
2.5 Diagrama de bloques de control en ángulos de euler y Thrust parte a. . . 24
2.6 Diagrama de bloques de control en ángulos de euler y Thrust parte b. . . 24
2.7 Comparación entre entrada escalón y respuesta para el ángulo Roll. . . . 26
2.8 Comparación entre entrada escalón y respuesta para el ángulo Pitch. . . . 26
2.9 Comparación entre entrada escalón y respuesta para el ángulo Yaw. . . . 26
2.10 Comparación entre entrada escalón y respuesta para el thrust (posición en
Z). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
2.11 Posición del drone en el eje X. . . . . . . . . . . . . . . . . . . . . . . . . 27
2.12 Posición del drone en el eje Y. . . . . . . . . . . . . . . . . . . . . . . . . 27
2.13 Modificación lazos de control para variables X, Y, Z . . . . . . . . . . . . 27
2.14 Comparación entre posicion deseada en eje X y comportamiento del prototipo 28
2.15 Comparación entre posicion deseada en eje X y comportamiento del prototipo 28
2.16 Ángulo Yaw del drone. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
2.17 Comparación entre señal de control y comportamiento del ángulo Pitch. . 29
2.18 Comparación entre señal de control y comportamiento del ángulo Roll. . 29
3.1 Sección transversal de un motor brushless de corriente continua. . . . . . 30
3.2 Circuito de control para motor DC brushless. . . . . . . . . . . . . . . . . 31
3.3 Ejemplo de wheelbase . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.4 SoC Cora Z7 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
3.5 SoC ESP32 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3.6 Frame ZMR250 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
3.7 Motores Emax RS2205 2300kV. . . . . . . . . . . . . . . . . . . . . . . . 36
3.8 Tabla de información motores Emax 2300 kV . . . . . . . . . . . . . . . . 37
3.9 Baterı́a LiPo 3S 2200 mAh 70C . . . . . . . . . . . . . . . . . . . . . . . 38
3.10 Hélices 5030 fabricadas en ABS . . . . . . . . . . . . . . . . . . . . . . . 39
3.11 ESC Littlebee 30[A]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
3.12 PDB Matek . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
3.13 IMU GY-BNO080 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
3.14 Barómetro BME280. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
3.15 Módulo GPS Ublox NEO-6M . . . . . . . . . . . . . . . . . . . . . . . . 41
3.16 Ultrasonido HC-SR04. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
3.17 Cámara Raspberry PI NoIR. . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.1 Ensamble simulado en 3d para validar tamaño de componentes. . . . . . 43
4.2 Representación fı́sica del ensamble simulado en 3d. . . . . . . . . . . . . . 44
4.3 Análisis de deflexión sobre la cubierta del UAV con 8 soportes. . . . . . . 45
vii
4.4 Análisis de deflexión sobre la cubierta del UAV con 6 soportes. . . . . . . 45

4.5 Separador de ABS para PDB. . . . . . . . . . . . . . . . . . . . . . . . . 46
4.6 Montaje de PDB y ESC’s. . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4.7 Montaje de motores. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
4.8 Placa de montaje para SoC Cora Z7 . . . . . . . . . . . . . . . . . . . . . 48
4.9 Montaje de shield y cubierta superior del prototipo. . . . . . . . . . . . . 48
4.10 Montaje estructural completo del prototipo solo con piezas originales del
frame. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.11 Soportes de aterrizaje originales de frame ZMR-250. . . . . . . . . . . . . 50
4.12 Diseño de soportes de aterrizaje con altura extendida. . . . . . . . . . . . 50
4.13 Soportes de aterrizaje impresos en 3D . . . . . . . . . . . . . . . . . . . . 51
4.14 Montaje estructural completo del prototipo con soportes de aterrizaje altos
(vista isométrica). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
4.15 Montaje estructural completo del prototipo con soportes de aterrizaje altos
(vista frontal). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
4.16 Soldadura de PDB . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
4.17 Soldadura de pin headers. . . . . . . . . . . . . . . . . . . . . . . . . . . 54
4.18 Shield y placa para montaje de sensores . . . . . . . . . . . . . . . . . . 55
4.19 Salida de lecturas de barómetro con código de ejemplo. . . . . . . . . . . 57
4.20 Salida de lecturas de IMU con código de ejemplo. . . . . . . . . . . . . . 59
4.21 Salida de lecturas de GPS con código de ejemplo. . . . . . . . . . . . . . 61
4.22 Esquemático de plataforma hardware para comunicación I2C. . . . . . . . 62
4.23 Módulo AXI PWM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
4.24 Diagrama de relaciones en comunicación ROS . . . . . . . . . . . . . . . 69
4.25 Diagrama de relaciones simplificado en comunicación ROS para el presente
proyecto . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
5.1 Plantilla de terminal de Ubuntu para monitoreo de comunicación ROS . 81
5.2 Lectura puerto serial Tarjeta Cora Z7 al inicio de la ejecución del código
de prueba. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
5.3 Lectura puerto serial Tarjeta Cora Z7 al estimar la altura base. . . . . . . 83
5.4 Lecturas del terminal Ubuntu para tópicos ROS en instantes iniciales de
la prueba experimental. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
5.5 Lecturas del terminal Ubuntu para tópicos ROS en instantes finales de la
prueba experimental. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
5.6 Capturas del resultado de la primera prueba de vuelo del prototipo. . . . 85
5.7 Muestra del montaje utilizado para las pruebas con compensación directa
sobre el giro de motores. . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
5.8 Capturas del resultado con compensación de valor 750 para corrección de
Roll y 0 para corrección de Pitch. . . . . . . . . . . . . . . . . . . . . . . 88
5.9 Capturas del resultado con compensación de valor 4650 para corrección de
Roll y 1300 para corrección de Pitch. . . . . . . . . . . . . . . . . . . . . 89
A.1 Diagrama de bloques de control en ángulos de euler y Thrust. . . . . . . 96
B.1 Diagrama de bloques del sistema con control de posición en X, Y y Z. . . 97
C.1 Plano de placa de montaje para SoC. . . . . . . . . . . . . . . . . . . . . 98
N.1 Diagrama de relaciones completo en comunicación ROS para el presente
proyecto . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182
Gonzalo Rojas Garcı́a viii

Índice de cuadros
1.1 Ventajas y desventajas por tipo de control . . . . . . . . . . . . . . . . . . . 11
ix
Acrónimos
ABS Acrylonitrile butadiene styrene
DC Direct Current
ESC Electronic Speed Controller
FET Field Effect Transistor

FPGA Field Programable Gate Array
GPS Global Positioning System
I2C Inter Integrated Circuits

IMU Inertial Measurement Unit
IRIM Instituto Renovetec de Ingenieria del Mante-
nimento
LiPo Lithium Polymer

LQR Lineal-Quadratic Regulator
MMA Motor Mixing Algorithm

MPC Model Predictive Controller
NMEA National Marine Electronics Association
PD Proportional-Derivative
PDB Power Distribution Board
PETG Polyethylene Terephthalate Glycol
PI Proportional-Integral
PID Proportional-Integral-Derivative
PLA Polylactic acid
PPM Pulse-Position Modulation
PPM Planed Preventive Maintenance
PWM Pulse Width Modulation
RAM Random Access Memory

ROS Robot Operating System
SHTP Sensor Hub Transfer Protocol

SoC System on a Chip
SPI Serial Peripheral Interface
UART Universal Asynchronous Receiver-Transmitter

UAV Unmanned Aerial Vehicle
USB Universal Serial Bus
Gonzalo Rojas Garcı́a x

VHDL Very High Speed Integrated Circuit (VHSIC)

Hardware Description Language
Gonzalo Rojas Garcı́a xi

1. Introducción
1.1. Introducción general
El mantenimiento es comúnmente entendido como el realizar reparaciones en la maquina-
ria cuando esta falla, o pasa un determinado tiempo o ciclo dado por el fabricante u obtenido
de forma empı́rica, como por ejemplo el realizar cambio de aceite en un automóvil cada 10000
km recorridos o el cambio de plumillas al notar que no limpian adecuadamente el parabrisas
respectivamente. No obstante, muchas veces las fallas se producen antes de cumplir dichos
periodos, ya sea por una mala operación del equipo o por defectos de fábrica en los com-
ponentes. Es por ello que se hizo necesario incluir dentro de las tareas de mantenimiento
herramientas que permitan prever el comportamiento de los diversos elementos en el equipo
y anteponerse a las fallas de estos.
Es ası́, como surge la inclusión de técnicas tecnológicas tales como el análisis de vibracio-
nes, análisis termográfico y análisis de partı́culas en aceites; ası́ como el uso de la inspección
visual, que, como lo define la revista inspectioneering [1], corresponde al método más básico y
antiguo de inspección y se realiza observando directamente el equipo en busca de indicadores
de falla. Se señala además que se requiere la experiencia del inspector para que el método
sea completamente efectivo. Un ejemplo del uso de este tipo de inspección, es la facilidad
para identificar fallas en tuberı́as, como se indica en un artı́culo de la empresa metalúrgica
pipemasters [2], que, si bien se indica el uso de esta inspección en el proceso de soldadura, se
puede extrapolar a la inspección en operación, como punto de partida para la identificación
de potenciales fallas, esto dada a la ventaja de un bajo costo en su realización permitiendo
enfocar los análisis de mayor complejidad y costo, solo en las zonas ya selecciónadas. Como
lo señala el Instituto Renovetec de Ingenieria del Mantenimento (IRIM), organización dedi-
cada a mejorar el mantenimiento en la industria [3], el mantenimiento predictivo involucra
la implementación de las técnicas anteriormente mencionadas para realizar un análisis que
permita predecir las fallas en un equipo. A pesar de ello, estas técnicas poseen tanto puntos a
favor, como en contra. Y por ello surge el problema a analizar mediante el presente proyecto.
Al enfocarse en 2 de los tipos de inspección declarados anteriormente (inspección ter-
mográfica e inspección visual), se puede establecer que si bien son métodos que se realizan
de forma distinta ambos tienen una limitante en común, la cual es que se debe ver de forma
directa en el punto a inspeccionar y, dependiendo de la resolución del instrumento (cámara
o cámara termográfica), se requiere de cierta cercanı́a a dicha zona.
Ası́, el problema que se presenta se puede resumir como la necesidad de acceder a zonas
remotas o de difı́cil acceso para el mantenedor, ya sea zonas donde se requiera un trabajo
en altura o lugares en los que el espacio existente es muy pequeño para el trabajo seguro,
mediante un equipo que permita realizar la captura de la información requerida para su
posterior análisis y toma de decisiones respectiva.
Es por lo anterior, que este proyecto busca presentar una solución a dicha problemática
empleando la tecnologı́a de vehı́culos aéreos no tripulados para realizar la toma de datos
resolviendo la dificultad de acceso para el mantenedor, como se detallará más adelante.
Es importante resaltar que, si bien la motivación, enfoque y justificación del presente
proyecto corresponde a la integración de una determinada tecnologı́a a un nuevo campo
de aplicación, dada la situación actual de contingencia sanitaria en la que se encuentra
el mundo, el proyecto considera en su totalidad el diseño del prototipo y solo parte del
Gonzalo Rojas Garcı́a 1

desarrollo, enfocándose en la parte inicial correspondiente a construcción, programación y

estrategia de control únicamente enfocada a la variable de altura dejando pendiente para
un futuro trabajo lo requerido para realizar a plenitud las tareas requeridas en el área de
mantenimiento. No obstante, esto se explicará mas a detalle en la sección de alcances y
limitaciones.
1.2. Estado del arte

1.2.1. Gestión de activos
Para comenzar, es importante definir que es la gestión de activos y su importancia en
la industria, término que sucede al mantenimiento puro integrando diversas herramientas
que permitan optimizar lo comúnmente desarrollado en el mantenimiento clásico integrando
bases de datos y análisis avanzados acordes con el desarrollo actual de la industria 4.0.
Según PAS 55 el concepto de gestión de activos consiste en el conjunto de actividades
y practicas sistemáticas mediante las que una organización maneja de manera óptima y
sustentable sus activos, contemplando su desempeño y riesgos a lo largo de su ciclo de vida
buscando cumplir su plan estratégico organizacional [4]. Esto quiere decir que abarca desde
el inicio de vida de un equipo pasando por su operación y mantenimiento hasta su fin.
Es por ello que dentro de la operación de los equipos, siguiendo esta metodologı́a, uno
de los puntos más importantes es el mantenimiento, el cual consiste en la aplicación de las
mejores prácticas de confiabilidad para incrementar el tiempo de operación de un equipo en
un entorno industrial [5].
Según [6] las estrategias de mantenimiento son empleadas para extender el ciclo de vida
de los equipos y prevenir su falla catastrófica durante la operación, mientras que por otro
lado según [7] mantenimiento puede ser definido como una combinación entre ciencia (por
la existencia de un estudio y metodologı́a), arte (porque depende la habilidad de quien lo
emplea) y por sobre todo una filosofı́a (que puede ser aplicada de forma intensiva, modesta o
no del todo). Esta práctica puede ser clasificada según tipos, en donde los principales (y más
conocidos) consisten en el mantenimiento correctivo, mantenimiento predictivo y preventivo
o Planed Preventive Maintenance (PPM) y el mantenimiento proactivo.
Como se mencionó anteriormente, de estos tres tipos de mantenimiento, el comúnmente
más asociado a dicho concepto consta en el mantenimiento correctivo, el cual, consta de
realizar las reparaciones a los equipos luego de que estos no puedan operar como consecuencia
de una falla, lo cual, si bien permite el uso de la vida útil de los equipos al máximo, muchas
veces genera pérdidas mayores a las producidas por realizar recambios o diagnósticos de
elementos previa falla, esto dado que dependiendo del componente, el tiempo medio para
reparar es muy alto.
Por lo anterior, surge el PPM que, según [8] el mantenimiento totalmente reactivo/co-
rrectivo no sigue la definición total de mantenimiento lo que busca proteger, preservar y
prevenir a los equipos de fallar. Por otro lado, se plantea el PPM como una inversión a
futuro sin perdidas mayores en equipamiento crı́tico [8]. También se plantea el PPM como la
rutina más importante que el personal de mantenimiento puede realizar. Por ello, además,
cabe destacar que finalmente el decisor más importante entre los tipos de mantenimiento es
el costo [7].
Dentro del PPM, se pueden encontrar diversas subcategorı́as entre las cuales se encuentra

el mantenimiento preventivo cı́clico (el cual puede ser basado en tiempo o en edad), el
mantenimiento preventivo basado en condición y el mantenimiento preventivo predictivo,
donde en los dos últimos uno de los puntos más importantes consiste en la inspección, donde
su principal diferencia radica que en el último se incluye el uso de estadı́stica. Fuera de esta
última distinción, se resalta la primordial importancia de la inspección, la cual puede ser
realizada de diversas formas (dependiendo de la variable que se desea monitorear).
Por otro lado, en paralelo, dichos tipos de mantenimiento pueden tener la caracterı́stica
de ser posibles en uso, lo cual dependerá, principalmente, del equipo y variable que se desea
controlar. Algunos ejemplos del mantenimiento que puede ser realizado en uso corresponde a
los valores entregados por manómetros en zonas de presión, indicadores de nivel en estanques,
métodos de verificación de rpm en motores, entre otros. Los cuales pueden ser verificados de
forma directa para luego ser registrados, catalogados dentro de la inspección visual. Según [3]
es un método sencillo y económico por lo que se recomienda realizar de forma frecuente para
tener una base de datos adecuada a las predicciones a realizar.
Dentro de las tareas, dos de los métodos comúnmente utilizados son la inspección vi-
sual y la termografı́a, puesto que dichos métodos permiten realizar inspección de forma
relativamente remota y permiten detectar problemas fácilmente mientras el equipo está en
funcionamiento. En el caso de la inspección visual, permite verificar grietas, fugas y otro tipo
de defectos que pueden ser vistos directamente por las personas. Por otro lado, la termografı́a
permite verificar las concentraciones de calor mediante una cámara con sensor térmico, lo
que permite ver las concentraciones de calor en ciertos elementos del equipo como tuberı́as
o motores y asociarlas a problemas comunes en estos como lo son el exceso de roce en una
tuberı́a de transporte o un motor que está siendo sobre esforzado por algún problema en el
sistema de transmisión o en sus espiras, entre otras posibilidades [3].
1.2.2. Vehı́culos aéreos no tripulados (UAV)

Un drone, palabra que proviene del inglés para zángano, tambien se utiliza para designar a
un vehı́culo (primordialmente aéreo) no tripulado destinado a realizar una tarea determinada.
Para el desarrollo de este informe se referirá a estos dispositivos con el sinónimo Unmanned
Aerial Vehicle (UAV) que se refiere directamente a los drone de tipo aéreo. Dentro de los
UAV se distinguen 2 tipos principales: los de ala fija, que se asemejan al funcionamiento
de un avión; y los de hélice rotatoria. En la imagen obtenida de [9], presentada en 1.1 se
puede ver un perfil superior de los tipos de UAV, cabe destacar que en la tercera columna
se presenta un tercer tipo correspondiente a los drone que integran un sistema hı́brido entre
dichos tipos principales.

Fig. 1.1: Diagrama de principales tipos de UAV’s
Respecto a los UAV de ala fija, presentan la ventaja de que debido a su aerodinámica
pueden permitirse un tiempo de vuelo más extenso en comparación de sus contrapartes
ahorrando el consumo de energı́a, pero tienen en contra la necesidad de ser impulsados de
forma manual para el despegue y no pueden mantener una posición fija en el aire hover. Por
otro lado, los UAV de hélice rotatoria, si bien presentan menor tiempo de vuelo, presentan
mayor versatilidad gracias a su capacidad de despegue autónomo y un movimiento más
preciso incluyendo la posibilidad de mantener su posición [10].
Este último tipo de UAV funciona usando el giro rápido de un rotor que impulsa el aire
hacia abajo permitiendo generar una fuerza que lo pueda elevar. Su versión más conocida
corresponde a los helicópteros, los que poseen un rotor principal para generar el impulso
hacia arriba y un rotor de cola que le permita rotar y producir un torque opuesto al del
rotor principal para lograr estabilidad. No obstante, su construcción requiere de elementos
complejos para controlar su posición como el swashplate que permite cambiar el ángulo
de ataque del rotor principal [11]. Es por ello que surge otro tipo de UAV que simplifican
dicho control mediante la adición de rotores a su construcción. Debido a ello existen los
tricópteros, cuadricópteros, hexacópteros y octacópteros, los que poseen 3, 4, 6 y 8 rotores
respectivamente [10]. Dentro de estos dispositivos, los más populares y comunes son los
cuadricópteros, los cuales mediante 4 inputs (o entradas), pueden controlar los 6 grados de
libertad del UAV (3 angulares y 3 lineales). En la imagen obtenida de [12], presentada en
Fig. 1.2, se presentan los distintos tipos de UAV de hélice rotatoria considerando número y
disposición de sus hélices.

Fig. 1.2: Diagrama de principales tipos de UAV’s de hélice rotatoria
Cabe destacar que, si bien, en la imagen no se presenta el UAV trihélice, su disposición

es similar a la del Hex Y, solo que posee sus hélices únicamente en la parte superior de
su frame. Por otro lado, es importante resaltar el hecho de que la selección de un tipo de
UAV se encuentra determinada bajo la premisa de “a más hélices, mayor estabilidad” pero
a su vez se tiene que “a más hélices, mayor cantidad de rotores y por ende mayor consumo
energético”. Es ası́, que el tricóptero, presenta el menor consumo de energı́a permitiendo un
menor peso (al disminuir el tamaño de su baterı́a) pero presentando una menor estabilidad
y complicando su control; en el lado opuesto, en caso de un octacóptero (8 rotores), se tiene
una estabilidad muy buena, incluso en situaciones de falla de un rotor, a costa de un mayor
peso por su alto consumo de energı́a, lo cual impacta altamente en su costo.
Otro punto que considerar sobre los UAV consiste en su forma de control, la cual se puede
clasificar en radiocontrolado o RC, supervisado o autónomo. Siendo la diferencia entre estos
el grado de independencia que posee el UAV, considerando completamente operado desde
tierra, operado de forma hibrida con parte autónoma y parte desde tierra, y finalmente de
modo completamente autónomo sin intervención de operador [13].
Los diversos tipos de UAV presentan aplicaciones en un amplio abanico de posibilidades
dependiendo de las necesidades de cada usuario. Por un lado, se tiene su uso en forma de
pasatiempo en un uso meramente recreativo, teniendo por un lado los drones orientados
a la captura de fotografı́as (en su mayorı́a cuadricópteros) utilizados para capturar, por
ejemplo, fotos de tipo selfie para grandes grupos de personas, siendo en su mayorı́a drones
con amplia estabilidad para la captura de imágenes de buena calidad. Otra aplicación en el
área recreativa corresponde al uso de drones denominados freestyle o de carreras, los cuales
entre sus caracterı́sticas principales se incluye el uso de una baterı́a de alto C-rating que
permite realizar movimientos a alta velocidad y la inclusión de una cámara FPV o de vista en
primera persona, permitiendo una mejor manipulación por parte del operador. Por otro lado,
se tienen los usos profesionales para los UAV entre los cuales se encuentran usos en áreas tan
diversas como la militar o el uso en la fotogrametrı́a, que consiste en el uso de las imágenes
(en este caso aéreas) para realizar mediciones y reconstrucción de mapas, como señala la
empresa dedicada a esto Aerial Insights [14]. Por otro lado, según la fundación de la energı́a
de la comunidad de Madrid [15] realizó una publicación directamente relacionada sobre las
distintas aplicaciones para los drones en el ámbito de la ingenierı́a civil, detallando usos
desde el área cartográfica con la recién mencionada fotogrametrı́a, hasta usos en medición de

la calidad del aire, exploración en minerı́a, seguridad y control de fronteras, mantenimiento

de redes eléctricas, entre otros; señalando además el tipo de UAV empleado en conjunto con
los sensores requeridos.
1.2.3. Estrategias de control

Al momento de realizar el control de un equipo, independientemente de su uso, se pueden
distinguir 2 grandes clasificaciones respecto a su estrategia de control. Por un lado, se tiene el
control en lazo abierto, el cual, como lo indica su nombre, no realiza una realimentación de su
estado por lo que se puede resumir como un comportamiento del sistema en estado manual
debido a que no se posee un cierto nivel de automatización para mantener las variables
monitoreadas en el valor deseado. Como se aprecia en [16], la salida no tiene influencia
directa en la entrada y, por lo tanto, solo funciona como método de control en los casos que
sea conocida la relación de entrada y salida de forma muy precisa.
Por otro lado, se encuentran los sistemas con control en lazo cerrado, los cuales, gracias
a la realimentación de estados, permiten realizar la automatización del equipo logrando
una respuesta en los elementos de entrada acorde con los cambios observados y registrados
respecto del estado del sistema en las variables de interés. Como se señala en [16], el control
de lazo cerrado, también llamado realimentado, utiliza la señal de salida (o una función de
esta) para realizar las acciones adecuadas y obtener la salida deseada. Entre algunos tipos
de control de lazo cerrado se pueden encontrar el control Proportional-Derivative (PD),
Proportional-Integral (PI), control en cascada, entre otros; cada uno con sus propias ventajas
y desventajas como se verá más adelante en el presente documento.
En particular, como se señala en [17], existe una serie de estrategias de control que son
preferidas para realizar el control de los UAV. Estas estrategias se pueden clasificar del modo
que se presenta en el diagrama obtenido de la misma fuente mostrado en la Fig. 1.3.

Fig. 1.3: Tipos de control más usados en UAV’s
De estas clasificaciones se tiene, en primer lugar, los controles de tipo lineal robusto,
los cuales corresponden a los tipos de control más simple dentro de esta clasificación, no
obstante, su resultado es suficiente para realizar la estabilización de un UAV. Dentro de esta
categorı́a se encuentran los controles de tipo Proportional-Integral-Derivative (PID), control
lineal cuadrático y control en el espacio de Hardy, conocido también como H∞ .
En el caso del control PID, como se señala en [18], corresponde a un control con retroali-
mentación negativa y, en donde el error es modificado mediante una constante proporcional
permitiendo evitar que el sistema sobre reaccione ante los cambios en la variable a con-
trolar. Dicho error también es modificado mediante una acción integral, cuyo objetivo es,
principalmente, lograr un error nulo en estado estacionario. Finalmente, se aplica una acción
derivativa, la cual permite aumentar la estabilidad del sistema en lazo cerrado realizando
una estimación del error en el futuro, corrigiendo la demora inherente al tiempo que tarda
en cambiar la salida respecto del cambio en la entrada. Entre las ventajas de este tipo de
control, se puede señalar su facilidad de implementación y sintonización, siendo esta última,
posible de realizar incluso de forma empı́rica. En contraste su principal desventaja, orientada
a la presente aplicación, corresponde a que el modelo más cercano al comportamiento real de
un UAV se trata de un sistema no lineal sub-actuado, por lo cual, al aplicar un control lineal
(como lo es el PID), se debe linealizar el sistema utilizando las ecuaciones de Euler-Lagrange.
Ası́, mediante este control, como se señala en [17], se logra un comportamiento adecuado
para estabilizar el UAV mientras la situación no difiera mucho del modo hover.
En cuanto al control lineal cuadrático, se profundizará en particular respecto al Lineal-
Quadratic Regulator (LQR), el cual, como se señala en [19], corresponde a una estrategia de
control que facilita la solución del problema dinámico para sistemas continuos, realizándolo
mediante una función de costo y formulándolo como un sistema lineal invariante respecto del
origen. Entre sus ventajas, como se señala en [17], es posible conseguir un seguimiento de la

referencia sin error estacionario. No obstante, entre sus desventajas se presenta la necesidad
de un modelo muy preciso, debido a que en caso contrario su desempeño se ve considera-
blemente disminuido, además de ser un tipo de control poco tolerante a las perturbaciones
externas.
Respecto del control H∞ , como se señala en [20], todos los sistemas tienen incertezas
asociadas, difı́ciles de medir y por lo tanto de modelar. Es por ello que se desarrolla el
control H∞ , el cual se centra en el desempeño y la estabilidad de un sistema, logrando un
mejor comportamiento en lazo cerrado respecto del control PID. Como se indica en [17], para
el uso de este control se plantea el sistema como un problema de optimización matemática.
Entre sus ventajas, se encuentra la capacidad de realizar estimaciones precisas a pesar de las
incertezas y perturbaciones con una saturación limitada de los actuadores. Por otro lado, su
desventaja radica en la complejidad de su implementación y su necesidad (en algunos casos)
de apoyarse en otro tipo de control, como lo es el predictivo.
Como se señaló anteriormente, el comportamiento de un UAV se puede modelar como un
sistema no lineal sub actuado, por lo cual, a continuación, se presenta una serie de controles
de tipo no lineal que se encuentran entre los más usados para el control de un UAV.
En primer lugar, se tiene el método de linealización por realimentación, el cual, como se
indica en [21], corresponde a la idea más práctica para el diseño de control no lineal. Como
se señala en [17], en este método se transforma el sistema no lineal en un sistema equivalente
lineal, y mediante esta transformación se produce una matriz no singular. Ası́, si bien este
método de control corresponde a un método no lineal para un sistema de este mismo tipo,
presenta desventajas que radican al ruido de sensado, lo cual provoca acciones de control
erróneas.
Por otro lado, se presenta el control de tipo backstepping, que como se presenta en [22], este
método permite resolver problemas de control robusto de forma poco restrictiva, consiguiendo
un error de seguimiento muy pequeño. En [17], se presenta este tipo de control como una
técnica que se construye con base a subsistemas que se pueden estabilizar mediante otros
métodos. Se parte de un sistema estable y se itera añadiendo nuevos controladores hasta
lograr la estabilización de todos los subsistemas. Como ventaja, este tipo de controlador
permite un control preciso en el área de los UAV y comparándolo con el control PID, reduce
el error de posición compensando la aceleración angular en forma inmediata. No obstante,
su implementación es más compleja respecto a dicho controlador.
Finalmente, en cuanto a los controles no lineales, se tiene el control de modo deslizante;
el cual, como se señala en [23], se define una superficie en la cual, el error se aproxima a
cero de forma asintótica. Este proceso, no tiene noción de economı́a y apunta únicamente a
la funcionalidad. En [17], se presenta que este control, al ser aplicado en UAV, presenta un
desempeño exitoso, no obstante, si bien su desempeño es mejor que el método de linealización
es menos preciso que el backstepping.
La última categorı́a por revisar corresponde a los tipos control catalogados como “in-
teligente”, los cuales poseen particularidad de cubrir un amplio rango de incertidumbre de
forma mucho mejor en comparación a las estrategias vistas anteriormente. Estos consisten
en control predictivo de modelo, control de lógica fuzzy y control de redes neuronales.
En el primer caso, el Model Predictive Controller (MPC), como se presenta en [24],
corresponde a un tipo de control en el que se realiza una predicción en un horizonte dado
para permitir el ajuste de la entrada del sistema. Este control es desarrollado mediante el uso
de herramientas de optimización, como lo es una función de costo. Como se señala en [17],

su ventaja radica en mantener la salida en las condiciones operacionales. No obstante, se

considera un método de control avanzado que requiere un modelo de predicción muy preciso
en conjunto a una estimación completa de los estados del modelo.
En [25] se presenta la lógica de tipo fuzzy como la descripción de un sistema, fuera de
la concepción de control clásico en donde se presenta una asignación binaria (1 o 0) para
determinar si un elemento pertenece o no a un definido conjunto universo; en el caso fuzzy
se presenta una serie de valores continuos a medida que se acerca a la delimitación. Como se
señala en [17], el control fuzzy se guı́a por una serie de expresiones “si-entonces”, en donde
la parte “si” corresponde a una región fuzzy del espacio de estados y la parte “entonces”
se refiere a la ley de control. La gran ventaja de este control radica en la facilidad para
entender el sistema desde un punto de vista de pensamiento humano (por su caracterı́stica
de continuidad) a diferencia de otros algoritmos. No obstante, su limitante, radica en la
definición del fuzzy set, el cual depende del diseñador. Las partes principales del diagrama
de control fuzzy se puede observar en la Fig. 1.4.
Fig. 1.4: Diagrama de bloques para controlador fuzzy
Finalmente, para el caso del control de tipo de redes neuronales, como se señala en [26],
corresponde a un tipo de control basado en inteligencia artificial y que, a diferencia del control
fuzzy se desarrolla siguiendo una base booleana precisa. Su principio de funcionamiento
radica en el uso de diferentes tipos de neuronas agrupadas en distintas capas, de las cuales
se distinguen 3 tipos, siendo la primera capa la de entrada, la última conocida como capa
de salida y finalmente las n capas de interconexión se denominan capas ocultas. Como se
señala en [17], cada red neuronal presenta una serie de valores conocidos como elementos de
procesamiento, los que presentan un parámetro de peso que determina su comportamiento.
Este tipo de control es altamente fiable en un amplio rango de incertezas, pero presenta
como desventaja la necesidad de encontrar los valores apropiados para los pesos mediante
el “entrenamiento de la red” lo que lo hace más complejo a nivel de implementación. Un
ejemplo de red neuronal se aprecia en la Fig. 1.5.

Fig. 1.5: Ejemplo de red neuronal
En base a lo expuesto anteriormente, es que se puede realizar una tabla resumen con
las principales ventajas y desventajas que presenta cada método de control orientado a su
implementación en los UAV, como se observa en 1.1.

Tipo de Control Ventajas Desventajas

PID Sencillo de implementar y A medida que se aleja del
sintonizar, control bueno enpunto de operación pierde
modo hover. efectividad, sensible a las
perturbaciones.
Control lineal Seguimiento de la referencia Requiere un modelo muy
cuadrático sin error en estado estacio- preciso, sensible a las per-
nario. turbaciones.
H∞ Realiza estimaciones preci- Alta complejidad de imple-
sas a pesar de las incertezas mentación a veces requiere
del sistema. el uso de control auxiliar.
Linealización Corresponde a un método Muy sensible al ruido de
por retroalimen- de control no lineal para un sensado.
tación sistema no lineal.
backstepping Control altamente preciso Mayor complejidad de im-
en el uso con UAV, presen- plementación que los con-
ta mejores resultados que el troles previamente descri-
PID tos.
Control de modo Al ser aplicado en UAV, Se centra solo en funciona-
deslizante presenta un desempeño exi- lidad, por lo que su utiliza-
toso. ción requiere mayor capaci-
dad de procesamiento.
MPC Mantiene la salida de for- Requiere de un modelo muy
ma adecuada en condiciones preciso en conjunto con el
operacionales. monitoreo completo de esta-
dos de dicho modelo.
Control fuzzy Se trata de un control de ti- La definición del fuzzy set es
po continuo, facilitando su arbitraria, dependiendo úni-
comprensión. camente del diseñador del
control.
Redes neurona- Altamente fiable en condi- Requiere de un adecuado
les ciones de incerteza. entrenamiento de la red pa-
ra obtener los pesos.
Tabla 1.1: Ventajas y desventajas por tipo de control
1.2.4. Discusión estado del arte

En base a lo expuesto anteriormente, del estado del arte se puede obtener la hipótesis
sobre la viabilidad de utilizar un UAV para realizar las tareas de inspección en la industria.
Esto, debido a la amplia versatilidad que entregan dichos dispositivos en cuanto al llegar a
lugares de difı́cil acceso para un ser humano.
Además, como se vio en cursos de mantenimiento, existen ubicaciones industriales, en
donde se producen fugas imperceptibles las cuales generan pérdidas y, en muchos casos, no
son alcanzables por los trabajadores, haciendo necesario el uso de andamios u otras formas
de inspección que al hacerse directamente son un potencial riesgo para su integridad fı́sica.

Es ası́ que, como se desarrolló previamente, gracias a las ventajas que permite un UAV, y
en particular, los de hélice rotatoria pudiendo realizar el hover en una posición determinada,
es que se decide determinar el uso de un cuadricóptero; esto, debido a que se considera que
es la mejor combinación en cuanto a estabilidad/peso para realizar la reducción de tamaño
necesaria considerando la aplicación de inspección de entornos cerrados, como lo son los
ambientes industriales. Ası́, se considera además, la versatilidad que permite uno de estos
dispositivos para soportar una cámara adecuada y, de esta forma, realizar la captura de
imágenes necesarias en la inspección.
En cuanto al desarrollo de estrategias de control, tomando en cuenta la aplicación y
etapas del proyecto que se describen en la presente memoria, es que se decidió utilizar como
método de control el de tipo PID. Esto debido a que, dadas las condiciones del proyecto,
se busca un control que permita realizar el adecuado posicionamiento del UAV y que a su
vez sea simple y barato (en términos de procesamiento) al momento de implementar. Cabe
destacar que, dado a que el presente proyecto considera el posicionamiento en torno a una
altura definida, no es problema el trabajo en torno al punto de operación definido para estos
efectos, permitiendo ası́ además una sintonización de forma empı́rica sin mayor dificultad.
1.2.5. Objetivo general

Construir un drone autónomo capaz de realizar tareas de inspección en mantenimiento
preventivo mediante una cámara.
1.2.6. Objetivos especı́ficos

Realizar un análisis matemático del comportamiento del drone.
Simular la respuesta dinámica del drone.
Diseñar un drone-prototipo que cumpla las caracterı́sticas necesarias para los objetivos
de inspección seleccionando elementos existentes en el mercado optimizando el costo
asociado.
Construir un prototipo para comprobar el funcionamiento del control y respuesta de

los sensores propuestos.
Implementar estrategia de control para el prototipo considerando el posicionamiento a

una altura determinada.
Realizar validación del prototipo mediante resultados empı́ricos respecto al posiciona-

miento de la aeronave en el eje Z (altura).
1.3. Alcances y limitaciones

1.3.1. Alcances
El desarrollo del presente proyecto contempla:
Justificación de su realización.

Modelamiento matemático y simulación del sistema en condiciones ideales.
Diseño de un prototipo que permita cumplir los requerimientos en una etapa experi-
mental, considerando tanto su control como la captura de imágenes.
Cotización y presupuesto requerido para construcción de dicho prototipo.
Construcción de un prototipo enfocado en validar la viabilidad de las estrategias de

control de posición, en particular de la variable altitud.
El UAV construido podrá realizar el posicionamiento a una altitud definida con el

soporte de un dispositivo remoto mediante intercambio de información vı́a Wi-Fi
1.3.2. Limitaciones
El presente proyecto está restringido a:
Si bien se busca el desarrollo de un UAV orientado a aplicaciones de inspección en zonas

interiores, siendo estas entendidas como lugares cerrados y con flujo de viento mı́nimo;
la etapa de construcción desarrollada y que se presenta a continuación corresponde
a un prototipo experimental, el cual, dadas sus caracterı́sticas fı́sicas, será puesto a
prueba en ambientes exteriores para la obtención de resultados.
Tiempos de vuelo de entre 10 a 15 minutos dadas las condiciones de autonomı́a pro-

vistas por la capacidad de las baterı́as disponibles.
A pesar de que se realiza la presentación de un diseño completo, considerando todos

los elementos necesarios para realizar el recorrido de rutas predefinidas, captura de
imágenes y evasión de obstáculos, la etapa de construcción y programación se enfocará
únicamente en permitir que el prototipo sea capaz de posicionarse a una altura deter-
minada, dejando los puntos restantes (correspondientes al posicionamiento en los otros
ejes, seguimiento de rutas y captura de imágenes) para un trabajo futuro. Esto se basa
principalmente, como se señaló con anterioridad, en la existencia de una contingencia
sanitaria durante todo el desarrollo del presente proyecto y, con ello, una gran limitan-
te respecto a las condiciones de trabajo en las que se desarrolla. Por nombrar algunas
dificultades se contempla los tiempos de compra de materiales, espacio y equipamiento
de trabajo, falta de un entorno controlado y seguro para la realización de pruebas de
vuelo, entre otros.
1.4. Metodologı́a
Para el desarrollo del presente proyecto se hará empleo de las siguientes metodologı́as:
1.4.1. Revisión bibliográfica

La revisión bibliográfica consiste en la recopilación y análisis de diversas fuentes de infor-
mación con el fin de establecer una base de trabajo para el desarrollo de una investigación o
proyecto. En el caso de este proyecto en particular se incluye la revisión bibliográfica puesto

que es necesario conocer los fundamentos de los UAV y el mantenimiento para ası́ facilitar
la integración de estas dos áreas en el desarrollo del prototipo presentado.
Por un lado, se consideró investigar sobre las bases, alcances y limitaciones de los UAV
para ası́ establecer la viabilidad del proyecto previa ejecución. Por otro, se consideró la
investigación sobre el mantenimiento como disciplina y la importancia de la inspección en
este para poder tener un fundamento solido sobre la factibilidad de llevar a cabo el proyecto.
1.4.2. Modelamiento y simulación matemática

El modelamiento y simulación matemática consiste en, mediante las ecuaciones de la
fı́sica y otras ciencias, realizar una aproximación al comportamiento real de un fenómeno
para predecir su comportamiento en condiciones reales. Para ello se emplea de herramientas
de cálculo matemático para simplificar el desarrollo de estas simulaciones.
En el caso del presente proyecto, dicha metodologı́a se utilizará para realizar un modelo
del comportamiento del UAV en condiciones ideales para posteriormente, mediante el uso
de un solver matemático, simular y realizar el análisis sobre el control propuesto además de
permitir obtener resultados a ciertas situaciones sin necesidad de realizar las pruebas con
un prototipo fı́sico, eliminando costos de prototipaje y riesgos al momento de hacer volar el
UAV.
1.4.3. Diseño y selección de componentes

En base a las simulaciones y condiciones de operación establecidas se requiere realizar
un proceso de diseño, el cual consiste, en una primera etapa, en realizar una evaluación y
selección de componentes adecuados para la aplicación, contemplando todas las funciona-
lidades requeridas para el proyecto. Posteriormente, luego de realizar la integración de los
componentes a modo de prototipo utilizando protoboard, se debe realizar el diseño corres-
pondiente de pcb (en este caso utilizando pcb pre-perforadas) para realizar el montaje de
todos los elementos al interior del prototipo al momento de su construcción.
1.4.4. Resultados experimentales

Esta metodologı́a se basa en el uso de resultados empı́ricos para realizar la validación
y/o comprobación de supuestos o de una hipótesis y ası́, obtener conclusiones respecto al
trabajo realizado.
En este caso, dicha metodologı́a será usada en la etapa final al momento de probar la
aplicación del control simulado en el prototipo fı́sico que se propone armar. Este método
será el decidor para validar la viabilidad del proyecto y permitirá justificar si es factible su
realización en una etapa futura buscando incluir el sistema de captura de imágenes, sistema
anticolisión y posteriormente reducir en tamaño para conseguir un dispositivo que cumpla
de forma óptima con todos los requerimientos de la aplicación.
1.5. Temario
El presente documento se encarga de detallar punto a punto el proceso de desarrollo para
el proyecto orientado al diseño y la construcción de un UAV prototipo orientado a realizar
tareas de inspección en mantenimiento. Es por ello que, posterior al análisis de estado del

arte como base para la fundamentación del proyecto, durante el capı́tulo 2 se explica en
detalle el desarrollo teórico de este, incluyendo los modelos matemáticos, seguidos de las
simulaciones. A continuación en el capı́tulo 3 se detalla el proceso de diseño. Posteriormente
en el capı́tulo 4 se desarrolla el proceso de construcción del proyecto. Ası́, en el capı́tulo 4
se presentan los resultados obtenidos del desarrollo completo del proyecto. Y finalmente, en
el capı́tulo 5 se presenta las conclusiones y el trabajo futuro requerido para el desarrollo del
proyecto.

2. Desarrollo teórico
Al momento de realizar una memoria de ingenierı́a, es fundamental realizar un análisis
teórico-matemático que permita justificar los aspectos más relevantes en relación con el
comportamiento de el o los fenómenos fı́sicos involucrados en la respuesta del sistema sobre
el cual se desea trabajar. Su principal objetivo consiste en la posibilidad de predecir el
comportamiento del sistema sin la necesidad de construirlo/modificarlo, para ası́ reducir
al mı́nimo el costo asociado a las tareas requeridas para completar los objetivos de dicha
memoria.
Es ası́ que, a continuación, se procede a realizar un análisis teórico, con el objetivo de
desarrollar un modelo que se aproxime a la realidad fı́sica del comportamiento de un UAV
en condiciones ideales. Ası́, con dicho modelo se procede a realizar, en la segunda mitad
del capı́tulo, simulaciones mediante un solver matemático, con el objetivo de validar las
estrategias de control planteadas para el desarrollo del proyecto considerando, en primera
instancia, el control sobre la altura y los ángulos de Euler que definen la posición del prototipo
y ası́, posteriormente, plantear una estrategia que permita a futuro desarrollar un control
sobre la posición del prototipo en el plano XY. Finalmente, los resultados del presente
capı́tulo sentarán las bases de la etapa de diseño que se plantea posteriormente.
2.1. Modelamiento matemático

Para el desarrollo de esta sección se consideró bibliografı́a de diversas fuentes y de cono-
cimientos previos de parte del autor. En cuanto a las fuentes, las principales son [27], [28]
y [29].
2.1.1. Sistemas de referencia

Para poder establecer un modelo matemático del prototipo es necesario precisar ciertos
puntos. En primer lugar, es importante dejar en claro la existencia de 2 sistemas coordenados
en su comportamiento, uno de ellos consiste en el sistema coordenado inercial, el cual existe
fijo a la tierra y es respecto al cual se definen las coordenadas absolutas. Esto se puede
apreciar de mejor modo en la figura Fig. 2.1.

Y
θ Φ
X
Fig. 2.1: Sistema coordenado inercial del drone.
Por otro lado, como se presenta en la figura Fig. 2.2, está el sistema coordenado del
UAV, que tiene las coordenadas fijas al “frame” o marco del prototipo, siendo Xb orientado
hacia el frente del prototipo entre los motores 1 y 2, Yb de forma perpendicular al eje Xb
separando los motores 2 y 3, y por último Zb de forma perpendicular a los ejes previos en
dirección hacia la parte superior del prototipo.
Zb
Yb
M3
M2
Xb
M4
M1
Fig. 2.2: Sistema coordenado del cuerpo del drone.

2.1.2. Comportamiento del UAV

La caracterı́stica principal de un drone cuadricóptero consiste en que, dadas 4 entradas
de movimiento (4 motores) se pueden controlar los 6 grados de libertad que posee el dispo-
sitivo. Es por ello que, para establecer su comportamiento, es necesario definir los puntos
que a continuación se detallan. En primer lugar, para establecer el comportamiento del UAV
es necesario definir la interacción entre los dos sistemas de referencia presentados anterior-
mente. Es por ello que se utilizará las matrices de rotación, las cuales permiten simular el
comportamiento del drone. Para este desarrollo se hará uso de la matriz de rotación por la
convención ZYX que corresponden con los ángulos de rotación en Roll (φ), Pitch (θ) y Yaw
(ψ). Lo cual se puede apreciar de mejor modo como sigue en las ecuaciones (2.1), (2.2), (2.3)
y (2.4). En donde C() representa los cosenos de los ángulos y S() los senos respectivos.
 
1 0 0
Rot(φ) = 0 Cφ −Sφ (2.1)
0 Sφ Cφ
 
Cθ 0 Sθ
Rot(θ) =  0 1 0 (2.2)
−Sθ 0 Cθ
 
Cψ −Sψ 0
Rot(ψ) =  Sψ Cψ 0 (2.3)
0 0 1
 
CθCψ CψSθSφ − SψCφ CψSθCφ + SψSφ
R =  SψCθ SψSθSφ + CψCφ SψSθCφ − SφCψ  (2.4)
−Sθ CθSφ CθCφ
Por otro lado, además, se puede establecer una matriz que permita relacionar la velocidad
angular del drone (ωφ , ωθ , ωψ ) con las velocidades angulares en los ángulos principales (Pitch
(θ), Roll (φ) y Yaw (ψ)) y viceversa. Esto se puede ver a continuación en las ecuaciones
(2.5) y (2.6), en donde es importante resaltar que además del uso de la notación S() y C()
para senos y cosenos respectivamente, se utiliza la expresión T() para hacer referencia a las
tangentes de los ángulos señalados.
    
φ̇ 1 SφT θ CφT θ ωφ
 θ̇  = 0 Cφ −Sφ   ωθ  (2.5)
Sφ Cφ
ψ̇ 0 Cθ Cθ
ω ψ
    
ωφ 1 0 −Sθ φ̇
 ωθ  = 0 Cφ CθSφ   θ̇  (2.6)
ωψ 0 −Sφ CθCφ ψ̇
En cuanto al comportamiento de los motores y su velocidad angular denotada por ωi se

presentan 2 reacciones. Por un lado, se genera una fuerza fi en la dirección del eje del rotor,
la que depende de una constante k o constante de levante. Por otro lado, dichas velocidades

angulares también producen un torque τM i , el cual depende del momento de su respectiva

constante de empuje b y el momento de inercia del motor. Las ecuaciones que muestran estas
relaciones son (2.7) y (2.8) respectivamente.
fi = k ∗ ωi2 (2.7)
τM i = b ∗ ωi2 + IM ∗ ω̇i (2.8)
No obstante, cabe destacar que, dado que el efecto de la aceleración angular del motor
tiene poca influencia en el torque, puede ser despreciado quedando como se ve en la ecuación
(2.9). Además, desde este punto en adelante se representará el cuadrado de la velocidad
angular de cada motor (ωi2 ) como Mi por simplicidad.
τM i = b ∗ Mi (2.9)
La fuerza producida por todos los rotores girando simultáneamente se puede expresar
como un vector denominado thrust en dirección axial al eje z y que está determinado por la
siguiente matriz (2.10).
   
0 0
T =  P0 = 0  (2.10)
4
k i=1 Mi M 1 + M 2 + M 3 + M 4
La rotación de los motores además produce 3 torques, en los 3 ángulos del sistema defi-
nidos como una matriz τB . Esto se define del siguiente modo en la ecuación (2.11).
   
τφ lk((M1 + M4 ) − (M2 + M3 ))
τb =  τθ  = lk((M1 + M2 ) − (M3 + M4 )) (2.11)
τψ M1 − M2 + M3 − M4
En donde l corresponde al brazo de palanca entre la aplicación de la fuerza y el centro de
masa del UAV. Además, se puede apreciar que en el caso del Roll los motores de un costado
aumentan su velocidad angular, mientras que los del costado opuesto la disminuyen. En el
caso del Pitch ocurre lo mismo, pero con los motores delanteros y traseros respectivamente.
Finalmente, en el caso del Yaw son los motores diagonalmente opuestos los que aumentan o
disminuyen su velocidad angular, es decir los motores M1 y M3 la aumentan, mientras que
los motores M2 y M4 la disminuyen.
Puesto que en el sistema inercial la fuerza centrı́fuga se anula, solo la fuerza gravitacional
y el thrust permiten definir la aceleración lineal del drone. Esto se ve como sigue en las
ecuaciones (2.12) y (2.13). Donde se ve que finalmente solo se utiliza la tercera columna de
la matriz de rotación para definir dichas aceleraciones.
     
ẍ 0 CθCψ CψSθSφ − SψCφ CψSθCφ + SψSφ
ÿ  =  0  + T  SψCθ SψSθSφ + CψCφ SψSθCφ − SφCψ  (2.12)
m
z̈ −g −Sθ CθSφ CθCφ

     
ẍ 0 CψSθCφ + SψSφ
ÿ  =  0  + T SψSθCφ − SφCψ  (2.13)
m
z̈ −g CθCφ
Otro punto para tener en cuenta es, dado que el drone se considera un dispositivo simétri-
co con sus cuatro brazos alineados, se tiene que la inercia respecto al eje x (Ixx ) es igual a
la inercia respecto al eje y (Iyy ). Además, en el cuerpo del drone, las aceleraciones angula-
res están definidas por la inercia en este, los torques τb asociados a este y las velocidades
angulares respectivas, quedando una expresión de la forma presentada en la ecuación (2.14).
     
ω̇φ A1 B1
 ω̇θ  = A2  + B2  ,
ω̇ψ A3 B3
Con :
(Iyy − Izz )ωθ ωψ
A1 =
Ixx
(Izz − Ixx )ωφ ωψ
A2 =
Iyy (2.14)
(Ixx − Iyy )ωφ ωθ
A3 =
Izz
τφ
B1 =
Ixx
τθ
B2 =
Iyy
τψ
B3 =
Izz
Es por ello que, en resumen, las ecuaciones que definen el comportamiento total del
sistema mediante sus estados constan de las siguientes:
Para la derivada de la posición se tiene las ecuaciones presentadas en (2.15)
x0 = ẋ,
y 0 = ẏ, (2.15)
z 0 = ż
Para la derivada de las velocidades lineales se tiene las ecuaciones presentadas en (2.16),
en donde se agrega el término de la resistencia del aire representado por Ax , Ay y Az .
(CψSθCφ + SψSφ)(M1 + M2 + M3 + M4 ) + Ax ẋ
ẋ0 = ,
m
(SψSθCφ − SφCψ)(M1 + M2 + M3 + M4 ) + Ay ẏ
ẏ 0 = , (2.16)
m
(CθCφ)(M1 + M2 + M3 + M4 ) + Az ż
ż 0 = −g +
m

Para la derivada de las posiciones angulares del prototipo respecto del marco de refe-
rencia inercial o terrestre, se tiene las ecuaciones presentadas en (2.17)
φ0 = ωφ + SφT θωθ + CφT θωψ ,

θ0 = Cφωθ − Sφωψ , (2.17)
Sφ Cφ
ψ0 = ωθ + ωψ
Cθ Cθ
Para la derivada de las velocidades angulares del prototipo respecto del marco de
referencia propio del UAV, se tiene las ecuaciones presentadas en 2.18
Lk((M 1 + M 4) − (M 2 + M 3)) Iyy − Izz

ωφ0 = − ωθ ωψ ,
Ixx Ixx
Lk((M 1 + M 2) − (M 3 + M 4)) Izz − Ixx
ωθ0 = − ωφ ωψ , (2.18)
Iyy Iyy
b(M 1 − M 2 + M 3 − M 4) Ixx − Iyy
ωψ0 = − ωφ ωθ
Izz Izz
2.1.3. Algoritmo de mezcla de motores

Una vez determinado los puntos anteriores, en base a las variables que se conocen del
comportamiento del drone el siguiente paso es definir el Motor Mixing Algorithm (MMA)
o algoritmo de mezcla de motores, cuyo fin es procesar señales provenientes de la etapa de
control y convertirlas a señales que se puedan enviar a cada motor para conseguir el giro a
una velocidad angular deseada. Para esto, se definen 4 bloques, donde su salida es enviada
directamente a cada motor (en el caso práctico, dichas señales son enviadas a los controla-
dores de velocidad de los motores, para que ejecuten el ajuste de velocidad de rotación en
cada motor).
Las ecuaciones que definen la señal para cada motor son las que se presentan a continua-
ción (2.19), en donde Tsig corresponde a la señal conseguida por el control en la posición en
Z o Thrust, φsig correspondiente a la señal resultante del control en el ángulo de Roll, θsig
correspondiente a la señal proveniente del control del Pitch y finalmente ψsig corresponde a
la señal obtenida en base al control del ángulo Yaw.
M1 = Tsig + θsig + φsig + ψsig ,

M2 = Tsig + θsig − φsig − ψsig ,
(2.19)
M3 = Tsig − θsig − φsig + ψsig ,
M4 = Tsig − θsig + φsig − ψsig
2.2. Simulación
Como se señaló anteriormente, en esta sección se presentará en detalle el proceso de simu-
lación en base a las ecuaciones obtenidas en el modelamiento desarrollado con anterioridad.
Para ello se comenzará detallando los parámetros requeridos en su desarrollo, seguidos de
una explicación de las estrategias de control a implementar, y ası́, finalizar con el detalle de
resultados en las diferentes condiciones de trabajo.

2.2.1. Parámetros
Para realizar la simulación los parámetros a considerar son los que se presentan a con-
tinuación. Realizando la salvedad que, debido a que es una etapa previa respecto de las
cotizaciones respecto al diseño del prototipo, se considerarán parámetros tı́picos, tomando
en cuenta el prototipo objetivo que se detallará más adelante. Ası́ se tiene:
Aceleración de gravedad: g = 9.81[ sm2 ]
Masa del drone: m = 0.468[kg], considerando un estimado en base a la masa tı́pica que
se tiene en los UAV más comunes en el mercado.
Distancia del centro de masa a los motores a lo largo del eje X o Y: l = 0.125cos(45◦ )[m],
dado que se considera un dron de 0.25 [m] de wheelbase, siendo este tamaño uno de los
más tı́picos entre las alternativas existentes.
Constante de levante del prototipo: k = 2.98 ∗ 10−6 , obtenido por las condiciones
estándar en cuanto a geometrı́a y caracterı́sticas de fluido propias del aire en condicio-
nes ideales.
Constante de empuje del prototipo: b = 1.14∗10−7 , estimado por la geometrı́a estándar

de los cuadricópteros y obtenido de la bibliografı́a citada anteriormente.
Inercia de los motores: IM = 3.357∗10−5 , considerando valor estándar para los motores
brushless tı́picos utilizados en UAV estándar con textitwheelbase de 0.25 [m].
Inercia del “frame” respecto a los ejes x e y: Ixx = Iyy = 4.856 ∗ 10−3 , estimado por la
geometrı́a (idealmente simétrica en el plano xy) del cuadricóptero.
Inercia del “frame” respecto al eje z: Izz = 8.801 ∗ 10−3 , estimado por la geometrı́a
tı́pica del prototipo.
Constantes de resistencia del aire: Ax = Ay = Az = 0.25, definido como valor tı́pico

para el aire a temperatura ambiente.
2.2.2. Controlador
Para el desarrollo del presente diseño, se considera en primera instancia un control de
tipo PD, el cual se utiliza en el control de los ángulos del UAV y la posición en eje Z.
Su funcionamiento se realiza realimentando dichas variables sensadas mediante la Inertial
Measurement Unit (IMU) y el barómetro y comparándolas con la referencia para obtener los
errores respectivos a corregir. El diagrama de bloques que representa este control se puede
apreciar en la figura 2.3.

Control PD
u(t) y(t)
e(t)
Kp
_
Kd d/dt
Fig. 2.3: Diagrama de bloques control PD.
Además del control anterior, se incluye uno de tipo PID para realizar el control sobre la
posición deseada en ejes X e Y, nuevamente utilizando la comparación de las variables sen-
sadas (esta vez incluyendo la señal obtenida por el sensor Global Positioning System (GPS))
y calculando el error respecto a la referencia. Cuyo diagrama se presenta a continuación en
la figura 2.4.
Control PID
Kd
u(t) y(t)
e(t)
Kp
_
Kd d/dt
Fig. 2.4: Diagrama de bloques control PID.
Cabe destacar que un punto importante al utilizar estos tipos de control es la necesidad de
realizar la sintonización adecuada de estos, para ası́ evitar la sobre atenuación o la oscilación
excesiva de la salida.
2.2.3. Simulación con control sobre Roll, Pitch, Yaw y Thrust

En primer lugar, para el desarrollo de esta simulación, se implementó el sistema de control
presentado en el diagrama de las figuras Fig. 2.5 y 2.6. Dicho diagrama se encuentra dividido
en 2 imágenes por su extensión, siendo sus conectores los literales de color verde A, B y C.
No obstante, en el anexo A se presenta la figura sin cortes, en una sola imagen de mayor
resolución.

No conectados Posición lineal

X
Z Y MUX A)
Z ref Thrust
PD
Control M1 a M4
φ ref PD Roll MMA B)

Control
θ ref PD Pitch
Control
ψ ref PD
Control Yaw
φ θ ψ
Posición angular
MUX C)
Fig. 2.5: Diagrama de bloques sistema con control sobre Roll, Pitch, Yaw y Thrust parte a) (lazos de
control).
A)
Posición lineal
M1 a M4 Velocidad lineal Posición lineal
B)
Comportamiento Aceleración lineal Velocidad lineal
del
UAV Velocidad angular Posición angular
Aceleración angular Velocidad angular
Posición angular
C)
Fig. 2.6: Diagrama de bloques sistema con control sobre Roll, Pitch, Yaw y Thrust parte b) (modelamiento
del UAV).
En las imágenes anteriores se pueden distinguir los siguientes puntos importantes:

Bloque de comportamiento del UAV: Este bloque contiene todas las ecuaciones fı́sicas
que describen el comportamiento del drone, relacionando las señales de entrada y salida
listadas a continuación:

Entradas:
• Señales de control para motores (M1, M2, M3, M4).

• Estados sensados de velocidades lineales (ẋ, ẏ, ż).
• Posiciones angulares (φ, θ, ψ).
• Velocidades angulares (ωφ , ωθ , ωψ )
Salidas:
• Derivada de la posición lineal (ẋ, ẏ, ż).

• Derivada de la velocidad lineal (ẍ, ÿ, z̈).
• Derivada de posiciones angulares (φ̇, θ̇, ψ̇).
• Dervada de velocidades angulares (ω̇φ , ω̇θ , ω̇ψ ).
Bloque integrador: Consiste en un bloque que permite integrar los estados para reali-
mentarlos a la etapa de control.
Etapa de control: Consta de los controladores PD encargados de realizar el control

sobre los ángulos de Roll, Pitch y Yaw, además del control de posición en Z.
Bloque MUX: Bloques multiplexores para realizar la separación de la señal en las

diferentes posiciones lineales y angulares.
Bloque MMA: Como se mencionó anteriormente, este bloque corresponde a la mezcla

de las señales de control para generar las salidas de los motores utilizadas en el control
del UAV.
Cabe destacar que el funcionamiento de esta simulación se comporta de forma similar

al control de un UAV mediante RC generando inputs para cada uno de los ángulos y para
el thrust de forma independiente. Ası́, los resultados que se obtienen al implementar este
modelo son los que se presentan a continuación:
En primer lugar, las 4 diferentes entradas al sistema que constan de Roll (φ), Pitch (θ),
Yaw (ψ) y Thrust (Z). Estas entradas se realizan de tipo escalón como se puede observar en
las figuras Fig. 2.7, 2.8, 2.9 y 2.10 en la curva de color rojo. Por otro lado, de color azul se
puede observar el resultado de dichas variables en el comportamiento del UAV.
Aquı́ se observa que, en el caso del Roll se inicia el escalón aproximadamente en el segundo
2 y permanece activo hasta el segundo 5.5 con una magnitud de π8 = 0.39[rad]. En el caso de
la referencia para el ángulo Pitch se contempla un escalón de igual magnitud que el anterior,
pero con duración entre los segundos 0.5 y 2.5 aproximadamente. En cuanto al ángulo Yaw
se mantiene en 0 puesto que no es muy representativo en este control. Para el Thrust, se
considera una magnitud de 10 metros en un comienzo, la cual se disminuye a 6 metros al cabo
de 8 segundos. Cabe destacar que la simulación se realizó para un intervalo de 10 segundos.
En cuanto a la respuesta se observa en primer lugar para el ángulo Roll que, en los
momentos donde el escalón cambia de valor en la entrada (2 y 5.5 segundos), se activa el
control PD estabilizándose en el valor correspondiente a π8 y 0 respectivamente. Lo mismo
ocurre con el ángulo Pitch, el cual presenta las curvas del control PD en los segundos 0.5 y

2.5. Finalmente, para el caso del ángulo Yaw, a pesar de tener su entrada en 0, se producen
oscilaciones en su valor, por lo que el controlador ejecuta una acción correctiva para mantener
su magnitud en 0.
Respecto al eje Z y los efectos del control PD sobre el Thrust, se puede señalar que se
puede ver una sintonización más precisa debido al control que se ejerce directamente sobre
la posición respecto al eje Z logrando una oscilación baja y un sobrepaso bastante aceptable,
aunque mejorable en etapas futuras del proyecto.
Señal de referencia vs comportamiento para el ángulo Roll Señal de referencia vs comportamiento para el ángulo Yaw
1 1
0.8 0.8
0.6 0.6
0.4 0.4
Ángulo (rad)
Ángulo (rad)
0.2 0.2
0 0
-0.2 -0.2
-0.4 -0.4
-0.6 -0.6
Señal de control Señal de control
-0.8 -0.8
Respuesta del sistema Respuesta del sistema
-1 -1
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
Tiempo (s) Tiempo (s)
Fig. 2.7: Comparación entre entrada escalón y Fig. 2.9: Comparación entre entrada escalón y
respuesta para el ángulo Roll. respuesta para el ángulo Yaw.
Señal de referencia vs comportamiento para el ángulo Pitch Señal de referencia vs comportamiento para Thrust
1 12
0.8 11
10
0.6
9
0.4 8
Distancia (m)
Ángulo (rad)
0.2 7
6
0
5
-0.2 4
-0.4 3
2
-0.6
1
-0.8 0
Respuesta del sistema Respuesta del sistema
-1 -1
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
Fig. 2.8: Comparación entre entrada escalón y Fig. 2.10: Comparación entre entrada escalón y
respuesta para el ángulo Pitch. respuesta para el thrust (posición en Z).
Finalmente es necesario presentar la posición del UAV en sus componentes lineales X e

Y. Para ello se presentan las figuras Fig. 2.11 y 2.12 respectivamente. Aquı́ es importante
destacar, puesto que no hay control de posición en los ejes X e Y, una vez existe algún ángulo
de movimiento tanto en el Pitch como en el Roll, se producirá un movimiento que persistirá
hasta que se logre la estabilización completa del UAV nuevamente. Además, cabe destacar
que la magnitud muy grande en el movimiento es causada por las ganancias definidas en el
control PD para lograr una muestra representativa de su comportamiento bajo este tipo de
control. Es por ello que se plantea la necesidad de sintonizar los parámetros de éste y, en el

prototipo real, considerar las limitaciones que presenta un motor en cuanto su velocidad de
giro.
Respuesta del prototipo en eje X Respuesta del prototipo en eje Y
1000 100
900 0
800 -100
700 -200
Distancia (m)
Distancia (m)
600 -300
500 -400
400 -500
300 -600
200 -700
100 -800
0 -900
-100 -1000
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
Fig. 2.11: Posición del drone en el eje X. Fig. 2.12: Posición del drone en el eje Y.
2.2.4. Simulación con control sobre posición X, Y y Z

En este caso, la simulación se realiza adaptando el modelo anterior agregando 2 etapas
nuevas lo cual se puede ver en detalle en la figura Fig. 2.13, considerando las mismas etiquetas
A, B y C para la conexión entre los lazos de control y el modelo matématico del UAV. No
obstante, el diagrama completo se puede ver en la sección de anexos B.
Posición lineal
MUX
X Y Z
A)
Z ref Thrust
PD
Control M1 a M4
Y relativo PID φ ref PD Roll MMA
Y ref Sat.
Control Control
X ref Rot ψ X relativo PID θ ref PD Pitch B)
Sat.
Control Control
ψ ref PD
Control Yaw
ψ
θ Posición angular
φ MUX
C)
Fig. 2.13: Modificación lazos de control para variables X, Y, Z
Dichas etapas consisten en:
Bloque matriz de rotación Yaw: Corresponde a un bloque que permite convertir la

posición de referencia del drone en el marco inercial a la posición respecto del sistema
coordenado del cuerpo del drone. Tiene de entradas las referencias deseadas en X e
Y, y el ángulo Yaw (ψ), y de salidas presenta la conversión de X e Y en el sistema
coordenado del drone.

Etapa de control de posición: Corresponde a 2 bloques de control PID que utilizan

las referencias obtenidas del bloque anterior para ejecutar la estrategia de control de
posición. Cabe destacar que dada la conversión entre la distancia en metros a un ángulo
en radianes es necesario un bloque de saturación Sat. que permita limitar el ángulo
entre los valores reales que se pueden obtener.
Ası́, aplicando lo anterior se tiene como resultado lo presentado a continuación.

En primer lugar, se considera una entrada de referencia de 2 para el eje X y de 6 para el
eje Y para ver el comportamiento del control respecto a dichas coordenadas. Además, cabe
destacar que en este punto no se presentara las señales correspondientes al posicionamiento
en el eje Z puesto que serı́an redundantes con lo visto en la sección anterior considerando la
misma referencia.
En cuanto al desarrollo de control de posición respecto a los ejes X e Y se presentan 3
entradas a la primera etapa de control, las cuales se pueden observar en 2.14, 2.15 y 2.16,
las cuales corresponden a la posición deseada en el eje X, eje Y y finalmente el ángulo Yaw
actual del sistema, dichas entradas se presentan nuevamente con la curva de color rojo.
Por otro lado, en azul, se presenta el comportamiento del prototipo ante la estrategia de
control. Cabe destacar que en cuanto al ángulo Yaw se presenta solo una curva que describe
el comportamiento del prototipo para dicha variable.
Aquı́ se puede observar, como se mencionó anteriormente que se busca el posicionamiento
del drone a 2 metros en el eje x desde el punto de partida. En cuanto al eje Y se desea una
posición a 6 metros. Y en el caso del ángulo Yaw se mantiene constante en 0 por lo que
tanto el sistema coordenado de referencia, como el sistema coordenado del frame del drone
se encuentran alineados, el módulo convertidor por matriz de rotación no tiene ningún efecto
por el momento.
En cuanto al comportamiento, se puede observar que el control presentado aproxima la
salida a un valor bastante cercano al deseado. No obstante se debe realizar una sintonización
más fina, la cual se realizara de forma empı́rica al momento de realizar la construcción y
pruebas de vuelo respectivas en el prototipo.
Señal de referencia vs comportamiento para posición en X Señal de referencia vs comportamiento para posición en Y
5 12
11
4 10
9
8
3
Distancia (m)
Distancia (m)
7
6
2
5
4
1
3
2
0 1
Respuesta del sistema 0 Respuesta del sistema
-1 -1
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
Fig. 2.14: Comparación entre posicion deseada en Fig. 2.15: Comparación entre posicion deseada en
eje X y comportamiento del prototipo eje X y comportamiento del prototipo

Comportamiento del ángulo Yaw

1
0.8
0.6
0.4
Ángulo (rad)
0.2
-0.2
-0.4
-0.6
-0.8
-1
0 1 2 3 4 5 6 7 8 9 10
Tiempo (s)
Fig. 2.16: Ángulo Yaw del drone.
Realizando un análisis más profundo, es necesario verificar las señales obtenidas en el

control PID de posición, que entrega las señales de Pitch y Roll requeridas para el sistema y
compararlas con los ángulos efectivos correspondientes. Esto se presenta en las figuras Fig.
2.17 y 2.18 respectivamente. Nuevamente utilizando la convención de rojo para las referencias
y azul para el comportamiento del UAV.
Aquı́ se puede observar que ambos ángulos poseen una respuesta oscilatoria, la cual se
atenúa al paso del tiempo. Por otro lado, es importante destacar que en ambos casos el
bloque de saturación solo actúa al comienzo, puesto que dada la sintonización del control, se
logró una respuesta que no se escapa de los lı́mites posibles en cuanto a la salida de control
( −π
3
a π3 ), por lo que se convierte el error presente en distancia a un valor angular de rotación
para lograr el control. Además, se puede detallar la necesidad de mejorar los parámetros del
control PD debido a la necesidad de seguir de forma más rápida la señal de referencia de los
ángulos respecto del comportamiento efectivo de estos, para lo cual se requiere de obtener
resultados empı́ricos del prototipo.
Señal de referencia vs comportamiento para el ángulo Pitch Señal de referencia vs comportamiento para el ángulo Roll
2 2
1.8 1.8
1.6 1.6
1.4 1.4
1.2 1.2
1 1
0.8 0.8
0.6 0.6
Ángulo (rad)
Ángulo (rad)
0.4 0.4
0.2 0.2
0 0
-0.2 -0.2
-0.4 -0.4
-0.6 -0.6
-0.8 -0.8
-1 -1
-1.2 -1.2
-1.4 -1.4
-1.6 -1.6
-1.8 Respuesta del sistema -1.8 Respuesta del sistema
-2 -2
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
Fig. 2.17: Comparación entre señal de control y Fig. 2.18: Comparación entre señal de control y
comportamiento del ángulo Pitch. comportamiento del ángulo Roll.

3. Etapa de diseño
En el presente capı́tulo se procede a detallar, en base al desarrollo teórico y bibliográfico
previo, el proceso de diseño del prototipo, justificando la necesidad y uso de cada compo-
nente a utilizar. Posteriormente, se detalla las cotizaciones de los componentes requeridos
disponibles en el mercado y sus principales caracterı́sticas.
3.1. Componentes necesarios para construccion de un UAV

En esta sección se detallará los materiales y elementos requeridos para la construcción del
prototipo en conjunto con su finalidad contemplando como objetivo la capacidad de realizar
las tareas de inspección. Estos son:
3.1.1. Motores brushless

Para proveer la fuerza motriz al prototipo se requiere del uso de un elemento que permita
convertir la energı́a eléctrica de la baterı́a en movimiento. Es por ello que se utilizara motores,
en particular del tipo brushless.
También llamados motores sin escobillas corresponden a un tipo de motor de corriente
continua que no requiere del uso de escobillas (conocidas también comúnmente por el nombre
de “carbones”) para su funcionamiento.
Los motores mas comunes de este tipo están compuestos por un estator que posee el
embobinado necesario para generar el campo electromagnético y un rotor que posee una
serie de imanes permanentes, los cuales al interactuar con el campo de la bobina producen
el torque requerido para la rotación. En la imagen Fig. 3.1 se aprecia un corte transversal a
la sección de un motor brushless con el detalle de sus partes [30].
Fig. 3.1: Sección transversal de un motor brushless de corriente continua.
Otra caracterı́stica importante que destacar es la necesidad de un controlador apropiado

para el uso de estos motores, el cual debe realizar una conversión de la corriente continua
a una señal trifásica que permitirá generar una oscilación en el campo electromagnético
permitiendo ası́ mantener una rotación constante y, dependiendo de la configuración de la
señal, ajustar la velocidad de dicha rotación al valor deseado para la aplicación. Este circuito
quedarı́a de la forma que se aprecia en la figura Fig. 3.2

Fig. 3.2: Circuito de control para motor DC brushless.
Además, es importante indicar que dependiendo de la parte móvil del motor brushless,
se pueden obtener dos tipos: los outrunner, en donde la parte móvil o rotor corresponde a la
zona exterior de este; y los inrunner, en los que el rotor corresponde a la parte interior del
motor.
3.1.2. Electronic Speed Controller (ESC)

Como se mencionó anteriormente, para realizar el control de los motores de tipo brushless,
es necesario de un controlador que permita realizar el cambio de corriente continua a una
de tipo trifásica que permita alimentar de forma sincronizada las bobinas del estator del
motor controlando ası́ su rotación. Es por ello que se utilizará controladores de velocidad
electrónicos o ESC los cuales permiten realizar este puente entre la alimentación Direct
Current (DC) y la entrada trifásica para los motores.
El control realizado por los ESC esta determinado por el funcionamiento de una serie de
transistores de efecto de campo (Field Effect Transistor (FET)), los cuales son controlados
a su vez por una señal, la cual puede ser de tipo Pulse Width Modulation (PWM) o Pulse-
Position Modulation (PPM) dependiendo de su configuración.
3.1.3. Controlador de vuelo

Para realizar la integración de todos los sensores, el control del UAV y la comunicación de
este con el dispositivo base (computador), es requerido un controlador de vuelo que permita
ser programado de tal forma que pueda responder a las señales recibidas, mientras inter-
cambia información de forma remota mediante comunicación inalámbrica con el computador
base, permitiendo ası́, desarrollar las rutas requeridas para la tarea a desarrollar.
Dadas las prestaciones que presenta, una buena alternativa es el uso de la tecnologı́a
System on a Chip (SoC). Dicha tecnologı́a, como su nombre lo indica (sistema en un chip
por sus siglas en inglés), corresponde a una tarjeta de desarrollo que permite la integración de
la tecnologı́a Field Programable Gate Array (FPGA) en conjunto con un microprocesador,
una tarjeta Random Access Memory (RAM) y las diferentes entradas y salidas que permiten,
como su nombre lo indica, integrar todo un sistema en un solo elemento.

3.1.4. Baterı́a Lithium Polymer (LiPo)

Para alimentar el sistema del UAV es necesario contar con una fuente de poder que
permita entregar grandes cantidades de corriente para los cambios repentinos en la rotación
de los motores y ası́ permitir un control adecuado del prototipo. Es por ello que se utilizará
una baterı́a LiPo.
Las baterı́as de tecnologı́a LiPo son dispositivos de almacenamiento que permiten una
mayor densidad de carga en comparación a las otras baterı́as entregando una mayor cantidad
de corriente con el mismo o incluso menor peso. Esto las hace idóneas para la aplicación de
UAV puesto que al ser necesario levantarse del suelo, se requiere tener el menor peso posible
con una carga lo suficientemente alta para tener una autonomı́a de vuelo adecuada.
3.1.5. Hélices
Las hélices o propelas corresponden a los elementos que permitan generar la fuerza de
empuje mediante la rotación de los motores.
En el caso de los UAV se pueden encontrar hélices de diversos materiales entre los cuales
se encuentran la fibra de carbono, plástico Acrylonitrile butadiene styrene (ABS), plástico
Polylactic acid (PLA), madera, entre otros, las cuales tendrán distintas caracterı́sticas en
cuanto a su calidad y duración en contraste con su coste.
3.1.6. Power Distribution Board (PDB)

La placa de distribución de energı́a o PDB corresponde al circuito que permite controlar
la alimentación para los diversos elementos que se encuentran conectados al sistema. Esta
tarjeta cuenta inicialmente con el input de corriente proveniente de la baterı́a y dependiendo
de la versión, 4, 6 u 8 salidas para ESC. Opcionalmente estas tarjetas pueden integrar
reguladores de voltaje a 3.3, 5 o 12 [V] para alimentar los circuitos de control que requieran
alimentación externa.
3.1.7. Frame
En un UAV el frame o marco, es la estructura de soporte para todos los componentes
del sistema, en otras palabras, es como el esqueleto que sustenta mecánicamente todas las
fuerzas involucradas al momento de levantar el drone del suelo.
Existen frames de muchos materiales, desde polı́meros (ya sea ABS, PLA u otro), ası́
como madera, metal, o materiales compuestos como la fibra de carbono. Como se señala en
un artı́culo de la empresa Matmatch, la cual se centra en el estudio de materiales [?], se indica
que, en el caso del frame para términos comerciales se prefieren materiales termoplásticos
como el nylon o el poliestireno que incluyen, entre sus caracterı́sticas, altas resistencias a
la tensión (100[MPa]) y baja densidad (2[g/cm3 ]); no obstante, se señala además, que para
usos industriales, al priorizar el desempeño, se utilizan materiales reforzados, como lo es la
fibra de carbono que a su vez presenta una relación entre resistencia y densidad mucho más
optima, pero a su vez su costo económico resulta mayor. Otro punto que considerar es la
denominada wheelbase que corresponde al diámetro de la circunferencia que conecta el centro
de todos los motores del UAV, y que define parámetros como el tipo de motor, tamaño de
hélice y peso máximo que se puede levantar, entre otros. Esta medida comúnmente se define

en milı́metros. En la imagen Fig. 3.3 se aprecia un ejemplo de la wheelbase para un UAV de

200 [mm].
Fig. 3.3: Ejemplo de wheelbase [31]
3.1.8. IMU
Corresponde a un elemento que posee los elementos necesarios para sensar el movimiento
del prototipo, ya sea en rotación o en traslación mediante acelerómetro y giroscopio, además
de contar con un magnetómetro para determinar la orientación del UAV de modo similar
a una brújula. Este elemento es parte fundamental al momento de realizar la estrategia de
control del UAV ya que permite obtener los inputs requeridos para realizar el ajuste de
posición en los ejes X e Y (en conjunto con el GPS), ası́ como los ángulos Roll, Pitch y Yaw.
3.1.9. Barómetro
Corresponde al dispositivo que permite, en base a la presión atmosférica, estimar la altura
aproximada del prototipo. Este elemento funciona utilizando de setpoint la presión a nivel
del mar para permitir estimar un cambio de presión en términos relativos y convertir dicho
diferencial en la altura efectiva del dispositivo.
3.1.10. GPS
Este elemento permitirá realizar el posicionamiento en coordenadas satelitales del proto-
tipo, esto a fin de permitir realizar un control en las posiciones X e Y. Su funcionamiento se
centra en el enlace del dispositivo con múltiples satélites diseñados para esta función a través
de una antena, permitiendo la triangulación del punto en el que se encuentra el dispositivo
con una tolerancia de alrededor de 5 [m] en promedio.
3.1.11. Sensor ultrasónico

Corresponde a un elemento que mediante el uso de ondas ultrasónicas permite detectar
objetos, además de calcular la distancia a la que estos se encuentran.

Para el prototipo, estos sensores permitirán identificar la presencia de obstáculos y ası́

permitir recalcular la ruta evitando las potenciales colisiones que se pueden producir en un
ambiente industrial.
3.1.12. Cámara
Elemento fundamental para la aplicación del prototipo, corresponde al módulo encargado
de realizar la captura de imágenes para su posterior revisión y análisis.
Este módulo puede ser tanto una cámara convencional, que permita ver fugas, trizaduras,
entre otras evidencias de posible falla, ası́ como una cámara termográfica, la que median-
te el uso del principio de emisividad de los cuerpos permite realizar un análisis sobre las
concentraciones de calor que puede presentar parte de la maquinaria a inspeccionar.
Este módulo sera utilizado para realizar la captura completa de datos que posteriormente
sera exportada para el análisis y toma de decisiones por parte del mantenedor.
3.2. Diseño y selección de componentes

En base a los materiales necesarios desarrollados en el punto anterior, se realizo el proceso
declarado a continuación para definir en detalle los componentes seleccionados acordes al
diseño del prototipo de etapa inicial propuesto para el presente proyecto, en conjunto con su
respectiva cotización. Para ello la serie de pasos realizados consistió:
En primera instancia, se fija como parámetro la utilización de una tarjeta SoC Cora Z7
para realizar el control. Esto, dada la facilidad para ser conseguida en los materiales dispo-
nibles en la universidad, su tamaño y peso acordes a la aplicación requerida, y por último
la capacidad de procesamiento y comunicación adecuadas para la utilización de sensores y
actuadores con protocolos plenamente conocidos y universales, como lo son Inter Integrated
Circuits (I2C), Serial Peripheral Interface (SPI), entre otros. Esta tarjeta en particular po-
see un procesador ARM-Cortex A9 de 667 Mhz integrado con una FPGA Xilinx serie 7 y
memoria RAM de 512 [Mb]. El coste aproximado de este componente es de alrededor de $
99 USD. En la figura Fig. 3.4 se puede ver la tarjeta Cora Z7.
Fig. 3.4: SoC Cora Z7.
Si bien la tarjeta Cora Z7 es la opción idónea para la aplicación buscada, presenta una
limitante, la cual corresponde a la carencia de elementos que permitan realizar comunicación
inalámbrica, considerando la necesidad de desligar de cualquier enlace fı́sico al prototipo

respecto del computador base. No obstante, dicha necesidad puede ser compensada mediante
la inclusión de una tarjeta ESP32.
La tarjeta ESP32 corresponde a un tipo de SoC de bajo y bajo poder, pero que en
compensación incluye de fábrica módulos Wi-Fi y Bluetooth integrados, lo que la hace un
componente ideal para el trabajo en conjunto con la tarjeta Cora Z7 consiguiendo potencia y
capacidad por un lado sumado a la comunicación inalámbrica por otro. Además, es importan-
te destacar la compatibilidad con comunicación I2C permitiendo establecer la comunicación
entre ambas tarjetas SoC. Otro punto favorable de la tarjeta ESP32 es su compatibilidad
con el IDE de arduino, por lo que a su vez, con las bibliotecas correspondientes, permite
integrar y generar un nodo Robot Operating System (ROS) para la comunicación entre el
prototipo y el computador base. Dicha interconexión y la operación de ROS sera detallada
más adelante.
El coste aproximado de este componente es de alrededor de $ 10 USD. En la figura Fig.
3.5 se puede ver la tarjeta ESP32.
Fig. 3.5: SoC ESP32.
A continuación, en base a ello se procedió a establecer el frame, el cual se debe seleccionar

en base a los valores tı́picos comerciales en wheelbase, de los cuales se considera como valor
adecuado el de 250 [mm]. Esto es debido a que si se considera un wheelbase de 220 [mm] se
tiene que el tamaño del centro del frame es muy pequeño para el circuito de control y no
serı́a capaz de volar adecuadamente. Por otro lado, con un wheelbase mayor (330 [mm]) se
considera un tamaño excesivo que, si bien tiene espacio suficiente para el control, requiere
mayor potencia en motores y baterı́a elevando el costo y no cumplirı́a el objetivo ideal de
reducir el tamaño del prototipo. Cabe destacar que existen frame con diversos wheelbase de
diámetros entre los señalados anteriormente, pero no son comunes en fabricación comercial,
dificultando su compra y elevando su costo.
Ası́, el frame seleccionado corresponde al ZMR-250, el cual es un frame de 250 [mm]
wheelbase, fabricado en fibra de carbono. Lo cual permite que sea ligero y a la vez resistente,

además de presentar un tamaño adecuado para la aplicación del presente proyecto. El costo
de este frame oscila en torno a los $ 26 usd.
En la figura Fig. 3.6 se muestra el modelo 3D del frame a utilizar.
Fig. 3.6: Frame ZMR250 [32].
Teniendo los parámetros anteriores como punto de partida, se puede realizar la selección
de motores de acuerdo con las recomendaciones presentadas por el fabricante del frame, las
cuales señalan el uso idealmente de motores de 2300 [kV] como actuadores, siendo esta la
velocidad en RPM por cada volt entregado al motor [33]. Por lo cual, considerando la mejor
relación funcionalidad/precio, es que se determina la factibilidad de usar motores Emax
RS2205 2300 kV, los cuales corresponden a motores de tipo outrunner que cumplen con la
caracterı́stica en [kV]. Cuyas dimensiones están determinadas por el numero 2205 (22 [mm]
de diámetro y 05 [mm] de altura permitiendo ser montados en el espacio dedicado para ello
en el frame. El costo de estos motores corresponde alrededor de $ 73 usd en total por el set
de 4 unidades.
Los motores son del tipo mostrado en la figura Fig. 3.7.
Fig. 3.7: Motores Emax RS2205 2300kV.
Ası́, el paso siguiente corresponde a la determinación de la alimentación, actuación y

control de los motores mediante la selección de baterı́a, hélice y ESC respectivamente. Para

ello, utilizará directamente la tabla proporcionada por el fabricante, la cual se encuentra

presentada en la figura Fig. 3.8
Fig. 3.8: Tabla de información motores Emax 2300 [kV] [34].
En dicha tabla, el primer criterio de selección corresponde a la decisión del sistema

de alimentación del prototipo, siendo posible baterı́a de 12 o 16 [V]. Por ello, nuevamente
tomando en cuenta la necesidad de reducir el tamaño del prototipo y además limitar su peso,
es que se considera adecuada una baterı́a LiPo 3S, la cual entrega voltaje nominal de 11.1
[V] y voltaje en carga máxima de 12.4 [V]. Además, tomando en cuenta la corriente máxima
utilizada por los motores a dicho voltaje corresponde a 23 [A], es que se decide utilizar una
baterı́a con C rating mayor a 45 y al menos 1800 [mAh] de capacidad para una autonomı́a de
vuelo aceptable. Tomando en cuenta dichas consideraciones y la disponibilidad de mercado
es que se considera el uso de una baterı́a 3S 70C 2200[mAh], la cual cumple adecuadamente
con la necesidad del sistema. Dicha baterı́a cuyo costo es de alrededor de $ 38 usd, se presenta
en la figura Fig. 3.9.
Fig. 3.9: Baterı́a LiPo 3S 2200 mAh 70C.
Posteriormente se define el tipo de hélice a utilizar, siendo opción hélices 5045 tripala
o bipala. Estas hélices poseen un largo de 5 pulgadas con paso de 4.5 pulgadas y su única
diferencia es la cantidad de palas que poseen. Por ello, considerando la disponibilidad de
mercado, se considera recomendable usar la versión de 2 palas fabricadas en ABS, pues
cumplen los requisitos de funcionamiento para el proyecto. Se requiere el uso de 2 hélices de
giro horario y 2 de sentido antihorario, cuyo costo por set es de alrededor de $ 8 usd. En la
figura Fig. 3.10 se presenta las hélices a utilizar.

Fig. 3.10: Hélices 5030 fabricadas en ABS.
Finalmente, de la tabla de motores se puede determinar el tipo de controladores ESC

necesarios para manipular la velocidad de rotación de motores. Para realizar esta selección,
el parámetro a considerar es la corriente máxima de circulación, que como se vio anterior-
mente corresponde a 23 [A]. Aplicando un factor de seguridad y considerando los valores
disponibles de mercado se considera adecuado el uso de ESC Littlebee de 30 amperes, cuyo
costo corresponde a alrededor de $ 26 usd por set y son presentados en la figura Fig. 3.11.
Fig. 3.11: ESC Littlebee 30[A].
Otro punto por considerar en la electrónica es la PDB, la cual, dadas las caracterı́sticas
ya descritas del sistema, debe permitir la alimentación de 4 ESC y la tarjeta SoC. Por lo
cual, en base a la disponibilidad de mercado, se considera como mejor opción la PDB Matek
que permite la conexión de hasta 6 ESC y posee además salidas de alimentación de 5 y 12
[V] de hasta 2 [A]. Dicha PDB tiene un costo aproximado de $ 12 usd.
En la figura Fig. 3.12 se puede observar la placa a utilizar.

Fig. 3.12: PDB Matek.
Por otro lado, en la selección de sensores, el proceso se describe a continuación:

Para la IMU, se consideró en primera instancia el módulo MPU-9250 dadas sus carac-
terı́sticas integrando acelerómetro, giroscopio y magnetómetro, siendo un módulo bastante
completo. No obstante, mediante investigación se consiguió el módulo BNO080 el cual inte-
gra las mismas caracterı́sticas del MPU, pero integra además un procesador ARM Cortex
M0 que permite obtener directamente los ángulos de Euler para realizar la estrategia de
control mediante una comunicación SPI o I2C con tensión de 3.3 [V]. Este módulo presenta
un costo de alrededor de $ 38 usd.
La IMU se puede ver en la Fig. 3.13
Fig. 3.13: IMU GY-BNO080.
Como se menciono anteriormente, existe la necesidad de agregar un módulo de sensado

de altitud de tipo barómetro. Para ello, considerando una tolerancia aceptable de ±1[m] el
módulo adecuado que cumple dichas caracterı́sticas corresponde al módulo BME-280 cuyo
costo corresponde a alrededor de $ 19 usd. Cabe destacar que este módulo cuenta además con
comunicación I2C y SPI con la inclusión además de un sensor de temperatura y humedad,
que, si bien no son utilizados en el proyecto, están presentes en la tarjeta. Este dispositivo
se puede apreciar en la Fig. 3.14.

Fig. 3.14: Barómetro BME280.
El siguiente sensor por seleccionar corresponde al módulo GPS, estableciendo el cual, dado
el presupuesto y disponibilidad en el mercado nacional, corresponde al módulo UBLOX-NEO
6M, que, si bien tiene una precisión adecuada, no es la versión mas pequeña en el mercado
para caracterı́sticas similares (MCI Módulo GPS SE868-A el cual se encuentra sin stock
actualmente). No obstante, cumple con los mı́nimos requeridos para la aplicación deseada
contando con comunicación de tipo Universal Asynchronous Receiver-Transmitter (UART)
y una pila de botón para almacenar configuraciones en memoria EEPROM. Su costo se
encuentra alrededor de $ 21 usd. Su PCB se puede apreciar en la Fig. 3.15.
Fig. 3.15: Módulo GPS Ublox NEO-6M.
Posteriormente, para realizar la detección de obstáculos se considerará el sensor ultrasóni-

co HC-SR04, debido a que tiene un bajo costo y una precisión adecuada a la aplicación
permitiendo realizar estimaciones de distancia empleando ecolocalización a través de ultra-
sonido. Este módulo tiene un costo de $ 5 usd. En la Fig. 3.16 se puede encontrar una imagen
referencial para este módulo.

Fig. 3.16: Ultrasonido HC-SR04.
Finalmente, para la cámara se considera el uso de un dispositivo con compatibilidad

con puerto MIPI, el cual esta especialmente diseñado para captura de imágenes. Ası́ se
encontró dentro de las posibilidades, considerando el elevado costo de las cámaras de tipo
termográfica, las cámaras Raspberry PI V2 en sus versiones con y sin sensor de infrarrojo.
Estos módulos cuentan con un sensor Sony IMX219 de 8 megapı́xeles, en donde la versión
NoIr (Sin sensor infrarrojo) presenta la ventaja de permitir captura de imágenes en lugares
de baja iluminación, como lo puede ser la industria, definiendo esta versión como la ideal
para el proyecto. Este módulo tiene un costo aproximado de $ 50 usd y su imagen referencial
se puede ver en la Fig. 3.17.
Fig. 3.17: Cámara Raspberry PI NoIR.
Es ası́, que en resumen el costo total del proyecto suma un total de aproximadamente $
425 usd, considerando los elementos mı́nimos requeridos para el funcionamiento del prototipo
y establecer su funcionamiento.

4. Etapa de construcción
En la presente sección se detallarán los pasos de construcción del prototipo incluyendo el
armado de componentes mecánico/estructurales, el conexionado electrónico y una sección de
programación explicando en detalle el funcionamiento de cada componente de forma aislada y
como se realiza su integración en el prototipo. Esta etapa incluye además pruebas focalizadas
en el funcionamiento de cada uno de los sistemas para tener referencias de su comportamiento
al momento de integrarlos permitiendo un proceso de depuración más sencillo.
4.1. Ensamble mecánico

En primer lugar, se realizó el montaje de la zona inferior del prototipo, con el fin de con-
firmar las dimensiones de la tarjeta Cora Z7 previamente obtenidas del manual de referencia,
respecto del tamaño del frame para realizar su ensamble. Previo a realizar la obtención de
materiales, se realizó un modelo CAD en 3D, para validar la factibilidad del montaje, como
se aprecia en la figura 4.1, en donde se simplificó el modelo de la tarjeta al de un para-
lelógramo (de color celeste), de dimensiones (102x58x15 mm) representando el largo, ancho
y espesor máximos de la tarjeta. Posteriormente se realiza la comparación con el montaje
real de la tarjeta observado en la figura 4.2, validando ası́ la posibilidad de montar la tarjeta
en la estructura.
Fig. 4.1: Ensamble simulado en 3d para validar tamaño de componentes.

Fig. 4.2: Representación fı́sica del ensamble simulado en 3d.
Como se ve anteriormente es requerido quitar 2 soportes verticales (resaltados en rojo

en el modelo 3D) para permitir el ensamble adecuado de la tarjeta en el frame, validando lo
simulado anteriormente. No obstante, se considera que la carga a la que se encuentra sometida
la zona superior del frame es mı́nima, por lo que la eliminación de dichos soportes no afecta
al desempeño del prototipo. Para realizar la validación de lo recién comentado, se realiza
una comparación (mostrada en las figuras Fig. 4.3 y 4.4) de la respuesta de la placa superior
ante una carga simplificada como uniforme sobre la cara superior de esta, correspondiente a
50 gramos (aproximadamente la masa correspondiente a la placa de sensores).
Es ası́ que se consideró ambos casos, por un lado, la presencia de los 8 soportes, en donde
se observa una deflexión máxima de aproximadamente 0,17[µm], como se observa en la figura
4.3; por otro lado, se realizó la simulación en el caso de quitar los 2 soportes que no permiten
la inserción de la tarjeta Cora Z7, situación en la que se observa una deflexión máxima de
aproximadamente 1.4 [µm], como se aprecia en la figura 4.4. Lo cual, si bien es 10 veces mayor
a la presente en el caso anterior, no constituye una deflexión lo suficientemente grande como
para ser considerada un problema al momento de implementar el diseño del prototipo. Cabe
destacar que ambas simulaciones fueron realizadas mediante el software Ansys 2021 R1 en
su edición gratuita para estudiantes, como se aprecia en la marca de agua de las figuras.

Fig. 4.3: Análisis de deflexión sobre la cubierta del UAV con 8 soportes.
Fig. 4.4: Análisis de deflexión sobre la cubierta del UAV con 6 soportes.
A continuación, una vez realizadas las conexiones de ESC’s a la PDB y conector respectivo
para la baterı́a (lo cual se detalla en la siguiente sección), se procede a realizar el montaje de
la PDB en el frame, utilizando para ello tornillos M3 y un separador fabricado en una placa
de ABS (la cual se puede ver en la figura 4.5), a modo de aislante eléctrico, fijando ası́ esta
pieza al frame. Además se realizó la fijación de los ESC’s mediante amarras plásticas para
cables. Esto se puede ver en la figura 4.6.

Fig. 4.5: Separador de ABS para PDB.
Fig. 4.6: Montaje de PDB y ESC’s.
Ası́, posterior a la programación del PWM requerido para el control de ESC’s y realizado
el conexionado de motores a cada ESC, validando el sentido de giro con las fases de alimen-

tación correspondientes, se realiza el montaje de los motores en sus respectivas posiciones.

Como se puede ver en la figura 4.7.
Fig. 4.7: Montaje de motores.
A continuación, previo a realizar el montaje de la tarjeta Cora Z7, es requerido realizar

un soporte, el cual cumple 2 funciones. Por un lado, permitir fijación mecánica de la tarjeta
al prototipo; por otro permite aislación dada la selección del material a utilizado, el cual
corresponde a polipropileno. Este material fue seleccionado dada su caracterı́stica de bajo
peso, se encuentra en láminas delgadas y la facilidad que provee a momento de trabajarlo,
además de ser aislante eléctrico cumpliendo las necesidades de la pieza. Para dicha fabricación
se realizó un plano a medida, de escala 1:1 el cual se puede observar en el anexo C. Una vez
realizado el plano, se realizó la impresión de este para ser usado de demarcación y finalmente
ser cortado desde la lámina de material inicial. El resultado se puede ver en la figura 4.8

Fig. 4.8: Placa de montaje para SoC Cora Z7
Una vez realizado dicho montaje y realizado el shield con las conexiones requeridas para
los sensores, se realiza su montaje en conjunto con la tapa superior del frame. Lo cual se
puede observar en la figura 4.9.
Fig. 4.9: Montaje de shield y cubierta superior del prototipo.
Finalmente, una vez que se tiene construida la placa shield de montaje para los sensores
y tarjeta ESP32, se realiza el montaje de esta en la zona superior mediante velcro adherida
utilizando cinta de doble contacto, finalizando ası́ el montaje del prototipo en el ámbito
mecánico, como se aprecia en la figura 4.10.

Fig. 4.10: Montaje estructural completo del prototipo solo con piezas originales del frame.
Al revisar el montaje descrito con anterioridad, se detecta el inconveniente correspondien-

te a la altura del prototipo respecto del suelo, la que, al ser muy pequeña, limita el espacio
para el posicionamiento de la baterı́a. Esto se debe principalmente al uso de los soportes
originales del frame, los que se pueden ver en la figura 4.11. Es por ello que se realiza una
búsqueda de alternativas de soporte para subsanar dicho problema, encontrando en el sitio
Thingiverse [35], encontrando un modelo en 3D con licencia no comercial “Creative Com-
mons” (presentado en la figura Fig. 4.12) que cumple con las necesidades de espacio para
situar la baterı́a sin riesgo de que esta toque el suelo, sobre todo en la situación de aterrizaje.
Cabe destacar que, si bien en la fuente existen diferencias entre los soportes delantero y tra-
sero, gracias a su simetrı́a en el montaje, es posible utilizar únicamente el diseño mostrado
correspondiente al soporte frontal en ambos extremos del prototipo.

Fig. 4.11: Soportes de aterrizaje originales de frame ZMR-250.
Fig. 4.12: Diseño de soportes de aterrizaje con altura extendida.
Ası́, se envió el modelo a un centro de impresiones 3D para ser impresas en Polyethylene

Terephthalate Glycol (PETG), dadas las caracterı́sticas de este polı́mero en cuanto a su

relación peso/resistencia, siendo además probada empı́ricamente por el creador del modelo.
Esta pieza impresa se puede observar en la figura Fig. ??.
Fig. 4.13: Soportes de aterrizaje impresos en 3D
Ası́, teniendo los nuevos soportes, se puede realizar el cambio de piezas, logrando el
armado final del prototipo a utilizar en el presente proyecto. Los resultados de la modificación
se encuentran evidenciados en las figuras Fig. 4.14 y 4.15, presentando tanto una vista
isométrica, como frontal del prototipo respectivamente. Un punto relevante a señalar es que
el prototipo, en su conjunto, presenta una masa de aproximadamente 695.64 [g].

Fig. 4.14: Montaje estructural completo del prototipo con soportes de aterrizaje altos (vista isométrica).
Fig. 4.15: Montaje estructural completo del prototipo con soportes de aterrizaje altos (vista frontal).
4.2. Conexionado electrónico

En cuanto a las conexiones electrónicas, el primer elemento a soldar corresponde a la PDB,
a la cual se le debió soldar en primera instancia el conector para la baterı́a y a continuación
la alimentación para cada ESC, ası́ como la alimentación para las tarjetas SoC. Para ello se

utilizó un cautı́n para soldadura electrónica a 325◦ C, con punta 900M-B y 900M-T para las
secciones de mayor y menor superficie respectivamente. La soldadura utilizada corresponde
a estaño para electrónica 60/40, y en el caso de las soldaduras de montura superficial se
aplicó flux en pasta para mejorar las terminaciones. Los parámetros de soldadura descritos
fueron los utilizados en las conexiones que serán descritas posteriormente. Dichas fijaciones
se aprecian en la figura 4.16.
Fig. 4.16: Soldadura de PDB
Posteriormente, se procedió a conectar los motores hacia cada ESC verificando el sentido
de giro de estos utilizando el código de generación de PWM.
Luego, se realizó la soldadura de los pin-headers o conectores de los módulos/sensores a
utilizar. El resultado es el que se puede ver en la siguiente figura Fig. 4.17

Fig. 4.17: Soldadura de pin headers.
Finalmente, una vez teniendo los pines definidos en la tarjeta Cora Z7 se procede a realizar
la soldadura del shield con las salidas correspondientes para la soldadura de la placa con
conectores encargada de soportar los sensores. Para la interconexión entre ambas se utilizó
cable plano AWG28 dado que permite flexibilidad y se puede encontrar en cintas de 10 o 40
vı́as lo que permite dejar la conexión de tipo bus. Esto se puede apreciar en la figura 4.18. Es
importante resaltar el uso de silicona termofusible para realizar el aislamiento de los puntos
en donde se realiza la soldadura y ası́ evitar que se produzcan cortocircuitos por factores
externos o incluso por la caracterı́stica conductora de la fibra de carbono que, si bien el frame
está construido con un compuesto de fibra con resina por lo que no es conductor al ciento por
ciento, aun existe el riesgo de que se generen falsos contactos que pueden desencadenar fallas
en el sistema. También, este adhesivo permite sujeción mecánica de los cables utilizados para
realizar los “puentes” entre los pines asegurando una mayor fiabilidad en su desempeño.

Fig. 4.18: Shield y placa para montaje de sensores
4.3. Programación y control

En esta sección se detallará el desarrollo del trabajo en programación requerido para
las distintas funcionalidades del prototipo y que, en su conjunto, finalmente componen el
controlador de vuelo diseñado para la aplicación presentada.
4.3.1. Verificación de módulos

En primer lugar, se realizó una verificación del funcionamiento de cada uno de los sensores
por separado, esto para validar la correcta conexión realizada con soldadura y que cada
elemento no tenga defectos de fabricación.
Dado que módulos de sensores están pensados para su uso con Arduino, y por lo tanto
sucede lo mismo con sus bibliotecas oficiales, es que se decide realizar esta comprobación
utilizando una tarjeta Arduino Mega 2560, elegida únicamente por su disponibilidad al mo-
mento de realizar el desarrollo puesto que se puede utilizar cualquier tarjeta de la familia
Arduino.
Al realizar estas comprobaciones, se tienen los resultados presentados a continuación para
el caso de cada módulo, en conjunto con las funciones de cada biblioteca utilizadas.
El primer módulo por revisar corresponde al barómetro BME280, el cual requiere el uso
de la librerı́a wire.h en el caso de realizar la comunicación mediante protocolo I2C. Además,
es importante señalar que este módulo corresponde a un sensor de ambiente, lo que significa
que integra un termómetro, barómetro y un higrómetro, de los cuales, dada la presente
aplicación, se despreciarán las variables de temperatura y humedad, focalizándose sólo y
únicamente en la presión y aun más especı́ficamente, en la altitud que se puede obtener
mediante dicha variable.
Es ası́ que, utilizando la biblioteca oficial para Arduino, desarrollada por la empresa
sparkfun [36], se realiza la comprobación mediante el ejemplo “Example1 BasicReadings.ino”
incluido en dicha biblioteca, con las modificaciones presentadas en el anexo D. No obstante,

a continuación, se detallará, a rasgos generales, los puntos y funciones más relevantes al

momento de utilizar el sensor.
En primer lugar, se incluyen las bibliotecas requeridas, siendo estas la librerı́a para Ar-
duino “Wire.h” correspondiente a las funciones para la comunicación I2C y la librerı́a del
sensor “SparkFunBME280.h”. Junto con ello se debe, además, definir un objeto con la clase
BME280 para almacenar las configuraciones y valores leı́dos por el sensor. El siguiente paso,
dentro de la función setup del programa, corresponde realizar la inicialización del bus I2C
en conjunto con inicializar las funciones y comunicación con el sensor, como se observa en el
fragmento de código presentado a continuación.
1 //Se inicializa el puerto I2C

2 Wire.begin();
3 //Se inicia y verifica la comunicación con el sensor
4 if (mySensor.beginI2C() == false)
5 {
6 Serial.println("The sensor did not respond. Please check
wiring.");
7 while(1); //Si falla se congela el programa
8 }
Una vez realizado aquello, y confirmando el éxito de la comunicación, se procede a rea-

lizar las lecturas de datos periódicas en la función loop en conjunto con su impresión por
puerto serial para realizar las verificaciones correspondientes, como se aprecia en el código
presentado a continuación.
1 //Se obtiene e imprime la presión en Pa

2 Serial.print(" Pressure: ");
3 Serial.print(mySensor.readFloatPressure(), 0);
4 //Se obtiene e imprime la altura en m
5 Serial.print(" Alt: ");
6 Serial.print(mySensor.readFloatAltitudeMeters(), 1);
Cabe destacar la importancia del uso de las funciones readFloatPressure y readFloatAltitude,

las cuales entregan en forma de datos de punto flotante la presión atmosférica y altitud sobre
el nivel del mar respectivamente.
Al utilizar dicho código se obtiene lo observado en el monitor serial de la figura 4.19, en
donde se observa una captura de valores en un intervalo de alrededor de 0.065 segundos, lo
cual corresponde a los 0.05 segundos entre iteraciones sumado al tiempo de comunicación
y de impresión de datos. Es importante destacar una variabilidad en el valor de altitud de
aproximadamente ±0.45[m], por lo cual es requerido realizar un promedio para estimar un
valor más preciso.

Fig. 4.19: Salida de lecturas de barómetro con código de ejemplo.
Ası́, a continuación, es requerido realizar el mismo proceso descrito anteriormente para

el barómetro, esta vez, orientado a realizar las pruebas y verificar las funciones existentes en
el uso de la IMU.
Es por ello que se realizó la descarga e instalación de la biblioteca de Arduino requerida
para la tarjeta BNO080 desarrollada por el fabricante [37]. Posteriormente, utilizando el
ejemplo “Example1-RotationVector.ino” y realizando algunas modificaciones que permitan
la lectura de no solo el vector de rotación, sino que obtener, lecturas de aceleración lineal y
velocidades angulares como se observa en el anexo E. No obstante, al igual que en el caso
anterior, se presentará los fragmentos de código más importantes en cuanto al procedimiento
de utilización del sensor.
En primer lugar, se debe realizar la inclusión de bibliotecas requeridas que, al igual que en
el caso previo, corresponden a la biblioteca “Wire.h” para comunicación I2C y la biblioteca
del sensor “SparkFun BNO080 Arduino Library.h”. Ası́, luego de definir un objeto con la
clase definida en la biblioteca (BNO080) y realizar la inicialización del bus I2C en la función
setup, se procede a realizar la inicialización del sensor, lo cual se realiza con el fragmento de
código presentado a continuación. Cabe destacar, el uso de la función “Wire.setClock” para
definir el relój del bus I2C, en este caso a 400 KHz.

2 if (myIMU.begin() == false)
3 {
4 Serial.println("BNO080 not detected at default I2C
address. Check your jumpers and the hookup guide.
Freezing...");
5 while (1); //Si falla se congela el programa
6 }
7 Wire.setClock(400000); //Incrementa el reloj I2C a 400 kHz

Posteriormente, aun en las configuraciones del programa, se debe definir los datos que se
necesitan leer, en conjunto con su frecuencia de muestreo. Para ello se emplean las funciones
mostradas en el fragmento de código que sigue.
1 myIMU.enableRotationVector(400); //Activa lecturas del

vector de rotación cada 400ms
2 myIMU.enableGyro(400); //Activa lecturas del giroscopio
cada 400ms
3 myIMU.enableLinearAccelerometer(400); //Activa lecturas del
acelerómetro lineal cada 400ms
Ası́, el siguiente paso corresponde a realizar las lecturas de la información de forma periódica
dentro de la función loop, como se observa en el fragmento de código a continuación usando
las funciones get*variable*, en donde *variable* corresponde a la variable a obtener.
1 //Verifica si hay información desde la IMU

2 if (myIMU.dataAvailable() == true)
3 {
4 //De ser asi almacena los valores recibidos
5 float quatI = myIMU.getQuatI();
6 float quatJ = myIMU.getQuatJ();
7 float quatK = myIMU.getQuatK();
8 float quatReal = myIMU.getQuatReal();
9 float gx = myIMU.getGyroX();
10 float gy = myIMU.getGyroY();
11 float gz = myIMU.getGyroZ();
12 float x = myIMU.getLinAccelX();
13 float y = myIMU.getLinAccelY();
14 float z = myIMU.getLinAccelZ();
15 }
Posteriormente se realiza la impresión de los valores obtenidos vı́a puerto serial para su
verificación. Los resultados obtenidos se pueden observar en la captura del monitor serial
mostrada en la figura 4.20. Aquı́, se observa una frecuencia de lectura de datos de alrededor
de 0.1 segundo, no obstante ,los datos presentan un cambio efectivo cada 0.4 segundos
aproximadamente dada la configuración de lectura cada 400 [ms].

Fig. 4.20: Salida de lecturas de IMU con código de ejemplo.
El último sensor a validar corresponde al GPS, el cual, a diferencia de los anteriores, solo
posee comunicación por protocolo UART. Un punto importante a destacar es el hecho de
que la comunicación de sensores de tipo GPS esta estandarizada por el protocolo National
Marine Electronics Association (NMEA) el cual se puede encontrar en más detalle en la
referencia [38]. Por ello, se requiere realizar una programación un tanto diferente, en la cual,
la biblioteca diseñada por el fabricante [39] funciona sólo como un traductor/intérprete de
los datos recibidos. Esto se puede ver de forma más intuitiva en el ejemplo “simple test.ino”,
el cual, siendo modificado para ser utilizado con el sensor seleccionado, queda de la forma
que se ve en el anexo F. Al igual que los casos anteriores, a continuación, se detallará paso
a paso los puntos más importantes del uso del sensor.
En primer lugar, se debe incluir las bibliotecas de comunicación y del sensor, siendo en
este caso, las librerı́as “SoftwareSerial.h” y “TinyGPS.h” respectivamente. Posteriormente
se crea un objeto con la clase TinyGPS incluida en la biblioteca y se definen los pines para la
comunicación UART. A continuación, dentro de la función setup, se realiza la inicialización
de la comunicación UART, que, dado el sensor a utilizar, se debe configurar a 9600 baudios.
Finalmente, dentro de la función loop, se debe realizar la lectura de la información del
sensor, en este caso, se realizará en intervalos de 1 segundo, interpretando mediante la fun-
ción “encode” cada uno de los caracteres recibidos e identificando si se ha leı́do una nueva
frase del protocolo NMEA como se aprecia en el siguiente fragmento de código.
1 // Durante 1 segundo se leen e interpretan los datos

entrantes
2 for (unsigned long start = millis(); millis() - start <
1000;)
3 {

4 //Mientras la comunicación esté disponible

5 while (ss.available())
6 {
7 //Se lee cada caracter
8 char c = ss.read();
9 if (gps.encode(c)) //Se interpreta el byte y se
verifica si entro una nueva frase
10 newData = true;
11 }
12 }
Ası́, posteriormente, en los casos de lectura de nueva frase, se realiza la obtención de los datos
desde las variables de almacenamiento de la clase TinyGPS como se aprecia en el fragmento
de código presentado a continuación.
1 //Si entró una nueva frase

2 if (newData)
3 {
4 //Se definen variables para almacenar latitud y longitud
5 float flat, flon;
6 unsigned long age;
7 //Se obtiene la posición y se almacena
8 gps.f_get_position(&flat, &flon, &age);
9 }
Finalmente se imprimen los valores obtenidos y las estadı́sticas de comunicación. Al imple-

mentar dicho código, el resultado obtenido en el puerto serial corresponde al observado en
la figura 4.21, en donde se aprecia de forma individual los valores de latitud y longitud,
además de la cantidad de satélites y precisión, correctamente extraı́dos de las frases en pro-
tocolo NMEA. Aqui, se observa una lectura cada alrededor de 1 segundo, con una pequeña
variación debido al tiempo de impresión de los datos por puerto serial.

Fig. 4.21: Salida de lecturas de GPS con código de ejemplo.
4.3.2. Transcripción de bibliotecas

Para utilizar la tarjeta Cora Z7, es necesario, en primer lugar, realizar la definición de la
plataforma de hardware a utilizar. Esto se realiza mediante el software Vivado proporcionado
por la compañı́a xilinx.
En dicha plataforma se debe indicar el sistema de procesamiento (el cual, en el caso de esta
tarjeta corresponde al denominado Zynq7), en conjunto con los distintos periféricos a utilizar,
los cuales se denominan AXI-xxxx, en donde xxxx corresponde al tipo de periférico que se
va a utilizar. Para el presente caso se debe el periférico AXI-iic, el cual contiene el soporte
y definiciones necesarias para el uso de comunicación I2C con los sensores de barómetro e
IMU. Cabe destacar que, debido a la forma de comunicación del GPS, se optó por realizar
su conexión y lecturas directamente con la tarjeta ESP32 dada su amplia compatibilidad
con los sistemas basados en Arduino y, en el caso de comunicación asincrónica (como lo es
UART) presenta un reloj compatible con los sensores diseñados para esta plataforma (16
MHz) por lo que su uso y programación serán detallados más adelante.
Una vez definidos los parámetros para el sistema de la tarjeta Cora Z7, el software
automáticamente realiza la conexión de todos los elementos generando un esquemático como
se aprecia en la figura 4.22.

Fig. 4.22: Esquemático de plataforma hardware para comunicación I2C.
Posteriormente se realiza el wrapper del sistema, que corresponde a un archivo en el

lenguaje de descripción de hardware seleccionado (Verilog o Very High Speed Integrated
Circuit (VHSIC) Hardware Description Language (VHDL)), de los cuales se decidió utilizar
el primero, dada la familiaridad del autor en cuanto a su uso. Este archivo actúa como un
resumen de las entradas y salidas del sistema, las cuales son relacionadas de forma interna
gracias a los perifericos AXI anteriormente descritos, simplificando su programación y ayu-
dando ampliamente al entendimiento del programa. Parte del código generado en el wrapper,
en donde se aprecia el resumen de entradas y salidas, es presentado a continuación.
1 module design_1_wrapper
2 (DDR_addr,
3 DDR_ba,
4 DDR_cas_n,
5 DDR_ck_n,
6 DDR_ck_p,
7 DDR_cke,
8 DDR_cs_n,
9 DDR_dm,
10 DDR_dq,
11 DDR_dqs_n,
12 DDR_dqs_p,
13 DDR_odt,
14 DDR_ras_n,
15 DDR_reset_n,
16 DDR_we_n,
17 FIXED_IO_ddr_vrn,
18 FIXED_IO_ddr_vrp,
19 FIXED_IO_mio,
20 FIXED_IO_ps_clk,
21 FIXED_IO_ps_porb,
22 FIXED_IO_ps_srstb,
23 shield_i2c_scl_io,
24 shield_i2c_sda_io);

En dicho código se observan 2 señales llamadas shield i2c scl io y shield i2c sda io, las
cuales corresponden a los inout (entradas/salidas) utilizados para las lineas SCL y SDA del
protocolo de comunicación.
Una vez definido aquello, se debe realizar el mapeo de puertos correspondientes con las
salidas del shield de la tarjeta, la cual presenta por diseño 2 pines dedicados a las lineas SDA
y SCL de la comunicación, por lo que se aprovecharan dichas conexiones.
El mapeo de puertos en esta tarjeta se realiza mediante un archivo .xdc, en el cual se
deben relacionar las variables definidas en el Esto se presenta en el código a continuación:
1 set_property -dict { PACKAGE_PIN P16 IOSTANDARD LVCMOS33 } [

get_ports { shield_i2c_scl_io }]; #IO_L24N_T3_34 Sch=ck_scl
2 set_property -dict { PACKAGE_PIN P15 IOSTANDARD LVCMOS33 } [
get_ports { shield_i2c_sda_io }]; #IO_L24P_T3_34 Sch=ck_sda
Una vez realizado dicho mapeo, se procede a realizar la transcripción las bibliotecas para
los sensores que utilizan comunicación I2C, cuyos códigos se pueden revisar en los anexos
G y H en el caso del barómetro y los anexos I y J para la IMU, en donde se presentan los
archivos .h y .cc correspondientes a la biblioteca de cada sensor. No obstante, a continuación,
se presentan los fragmentos de código más relevantes que corresponden a las modificaciones
necesarias para realizar cada tipo de comunicación. Esta transcripción, ası́ como toda la
programación en C/C++ del proyecto requerida para la tarjeta Cora Z7, se realiza mediante
el software Xilinx SDK incluido con la suite de Vivado.
En primer lugar, dado que se trabajará con ambos sensores utilizando la comunicación
I2C, hay cambios generales en común que se pueden detallar. El primero de ellos corresponde
a la definición de 2 parámetros referentes al AXI-IIC correspondientes a la identificación del
dispositivo AXI y su dirección base, los cuales son requeridos al momento de utilizar las
funciones de dicho dispositivo. Estos parámetros se muestran a continuación.
1 //Definiciones para axi iic

2 #define IIC_DEVICE_ID XPAR_IIC_0_DEVICE_ID
3 #define IIC_BASE_ADDRESS XPAR_IIC_0_BASEADDR
Otro cambio en general realizado para ambas bibliotecas corresponde a la modificación de
las clases BME280 y BNO0080 para retirar las funciones orientadas a la comunicación SPI,
debido a que el objetivo del presente proyecto se centra en el uso de ambos sensores úni-
camente por comunicación mediante el protocolo I2C. Por otro lado, el cambio principal a
realizar en cuanto a la comunicación corresponde al método de envı́o y recepción de datos.
En el caso de envı́o, la secuencia de Arduino es como sigue.
1 puertoI2C->beginTransmission(Dirección); //Secuencia de
inicio
2 puertoI2C->write(Dato1); //Dato número 1
3 puertoI2C->write(Dato2); //Dato número 1
4
5 puertoI2C->write(DatoN); //Dato final

6 puertoI2C->endTransmission(); //Secuencia de término

En donde se envı́a de forma manual la secuencia de inicio en conjunto con la dirección del
esclavo, luego los datos uno a uno y finalmente se envı́a la secuencia de término (STOP). En
cambio, al utilizar un dispositivo AXI en la tarjeta Cora Z7, el algoritmo de envı́o es el que
se muestra a continuación.
1 u8 bufferEnvio[N]; //Vector de datos a enviar

2 for(i = 0; i<N; i++){
3 bufferEnvio = Dato[i]; //Escritura de datos a enviar
en el buffer
4 }
5 StatusReg = XIic_ReadReg(IIC_BASE_ADDRESS, XIIC_SR_REG_OFFSET
); //Verificación del estado del bus
6 //Si el bus esta libre
7 if(!(StatusReg & XIIC_SR_BUS_BUSY_MASK)) {
8 XIic_Send(IIC_BASE_ADDRESS, Dirección ,
bufferEnvio, N, estado futuro del bus); //
La función send agrupa la secuencia de
inicio, datos y termino.
En donde se requiere almacenar previamente los datos en un vector de envı́o, para poste-
riormente verificar si el bus de comunicación se encuentra libre y, de ser ası́, realizar el envı́o
de datos mediante una función llamada Xiic Send, cuyos argumentos son la dirección base
del dispositivo AXI, la dirección del esclavo, el buffer de datos a enviar, la cantidad de datos
contenidos en el buffer y finalmente una señal que indique si se desea enviar una señal de
STOP o si se requiere mantener retenido el bus (también llamado estado HOLD) el bus para
evitar se realice envı́o de datos por parte de otros dispositivos conectados a dicho bus. Ası́, un
ejemplo práctico en donde se utiliza este algoritmo corresponde al mostrado a continuación,
extraı́do de la biblioteca BNO080 correspondiente al sensor IMU, en donde es importante
resaltar el uso del protocolo Sensor Hub Transfer Protocol (SHTP). Este protocolo consiste
en un método estándar para realizar la transferencia de datos desde un sensor, ya sea por co-
municación I2C o SPI. Su particularidad radica principalmente en el envı́o de una cabecera o
header de 4 bytes con información de formato respecto al paquete de datos a transferir. Esta
cabecera consiste en utilizar los primeros 2 bytes para indicar el largo del paquete a enviar,
mientras que el tercer byte indica el canal de comunicación, es decir, le da un sentido a la
información a enviar, distinguiendo si es información de configuración o lectura del sensor.
En cuanto al cuarto byte, corresponde al valor de secuencia de la comunicación por el ca-
nal previamente indicado y ası́ permitir la comunicación sincronizada y adecuada del módulo.
1 buffer2send[0] = (packetLength & 0xFF);

2 buffer2send[1] = (packetLength >> 8);
3 buffer2send[2] = channelNumber;
4 buffer2send[3] = sequenceNumber[channelNumber]++;
5

7 //Almacena en el buffer de envı́o los bytes de datos

8 for (u8 i = 0; i < dataLength; i++)
9 {
10 buffer2send[4+i] = shtpData[i];
11
12 }
13
14 //Si el bus I2C no esta ocupado, envı́a el buffer

15 StatusReg = XIic_ReadReg(IIC_BASE_ADDRESS,
XIIC_SR_REG_OFFSET);
17 checkpoint = XIic_Send(IIC_BASE_ADDRESS,
BNO_ADDRESS , buffer2send, sizeof(
buffer2send),XIIC_STOP);
18 }
Para el caso de recepción de datos, también hay cambios en el algoritmo. En el ca-

so de Arduino, el algoritmo de recepción de datos es como se presenta a continuación, en
donde mediante la función “requestFrom” se realiza la solicitud de N cantidad de bytes al
esclavo de dirección “Dirección”, luego se verifica que el esclavo mantenga el bus activo para
enviar los datos y finalmente, se lee dato por dato hasta realizar la lectura de todos los datos.
1 puertoI2C-> requestFrom(Dirección, N); //Se solicita mediante

la dirección del esclavo una determinada cantidad de bytes
2 if (waitForI2C() == false)
3 return (false); // Se verifica que el esclavo envı́e datos
4 //Se lee dato por dato y se almacena en una variable
5 Dato1 = puertoI2C->read();
6 Dato2 = puertoI2C->read();
7
8 DatoN = puertoI2C->read(); // Último dato leı́do
En contraste a lo anterior, para realizar dicha transferencia de datos en la tarjeta Cora

Z7 el algoritmo es el que se detalla a continuación, en donde se define, al igual que en el
envı́o de datos, un buffer que contiene, en este caso, el espacio de memoria reservado para
almacenar los datos a recibir. Y ası́, mediante la función “XIic Recv” se realiza la solicitud
y lectura de datos de forma directa, entregando como argumentos de la función la dirección
base del dispositivo AXI, la dirección del esclavo, el número de bytes a leer y el estado en el
que se desea dejar el bus al terminar la recepción.
1 u8 bufferRecibo[N]; //Vector de datos a recibir

2 XIic_Recv(IIC_BASE_ADDRESS, Dirección ,bufferRecibo, N,
estado futuro del bus); //La función recv agrupa la
secuencia de inicio, datos y término.

Un ejemplo del uso de la lógica descrita anteriormente corresponde a la lectura de datos de

un registro en particular en el sensor barométrico. Función en la cual se utiliza el algoritmo
de envı́o para indicar el registro a leer y posteriormente el algoritmo de recepción para leer
los datos solicitados y almacenarlos. Esto se ve en el fragmento de código mostrado a conti-
nuación.
1 //Se verifica disponibilidad del bus

3 //Se envia la dirección de registro a leer
BME_I2C_ADDRESS , buffer, sizeof(buffer),
XIIC_STOP);
6 }
7 //Se solicita el bit requerido
8 check2 = XIic_Recv(IIC_BASE_ADDRESS, BME_I2C_ADDRESS
,&result, numBytes, XIIC_STOP);
4.3.3. Generación de PWM

En este caso, para el desarrollo de un módulo PWM es necesario realizar un nuevo
módulo AXI, el cual se debe configurar en Verilog realizando la lógica requerida en la parte
PL(programable logic) de la tarjeta y ası́ conseguir una señal PWM variable controlada por
código.
Es importante tener en consideración que los ESC’s trabajan con señales PWM a una
frecuencia de 60[Hz] y con un duty cycle entre 6 % y 12 %. Es por ello que para realizar dichas
modificaciones se toma el reloj principal de 50[Mhz] para ası́ tener una resolución bastante
fina al momento de realizar el control.
Para definir este periférico se utilizó los códigos presentados en los anexos K y L. Aquı́
los puntos más importantes corresponden al uso de un divisor de reloj con un bus de cuenta
de 20 bits para llegar a una cuenta de 833334 lo que permite realizar la división del reloj a
la frecuencia requerida. En este periférico, el input corresponde al valor de una cuenta que
permite definir el duty cicle con un valor entre 0 y el máximo recién mencionado. Esto en
código se aprecia de mejor modo en el fragmento presentado a continuación.
1 reg [19:0] counter = 0;
2
3 //contador
4 always @(posedge S_AXI_ACLK) begin
5 if(counter < PWM_COUNTER_MAX-1)
6 counter <= counter + 1;
7 else
8 counter <= 0;
9 end
10

11 /*Si el dato leı́do (slv_reg) es menor a contador entrega

12 0 en la salida, de lo contrario 1*/
13 assign PWM0 = slv_reg0 < counter ? 1’b0 : 1’b1;
Cabe destacar que el PWM generado posee 4 salidas permitiendo tener una señal indepen-
diente por cada ESC, como se aprecia en la figura 4.23.
Fig. 4.23: Módulo AXI PWM
Ası́, posteriormente se realiza el mapeo de puertos mediante el archivo .xdc enlazando

las variables PWM0-PWM3 a los pines 28 a 31 respectivamente. Esto se ve en el código
presentado a continuación.
1 set_property -dict { PACKAGE_PIN U13 IOSTANDARD LVCMOS33 } [

get_ports { PWM0 }]; #IO_L3P_T0_DQS_PUDC_B_34 Sch=ck_io[28]
2 set_property -dict { PACKAGE_PIN V15 IOSTANDARD LVCMOS33 } [
get_ports { PWM1 }]; #IO_L10P_T1_34 Sch=ck_io[29]
3 set_property -dict { PACKAGE_PIN T16 IOSTANDARD LVCMOS33 } [
get_ports { PWM2 }]; #IO_L9P_T1_DQS_34 Sch=ck_io[30]
4 set_property -dict { PACKAGE_PIN U17 IOSTANDARD LVCMOS33 } [
get_ports { PWM3 }]; #IO_L9N_T1_DQS_34 Sch=ck_io[31]
Una vez se tiene el mapeo de puertos, es posible realizar el código de control en el software
Xilinx SDK.
Para modificar el valor de los registros para el control del duty cicle mediante código en
Xilinx SDK se utiliza la función Xil Out32(*reg*, *val*) en la cual se indica el registro a
escribir en la variable *reg* y el valor a escribir en la variable *val*, como se presenta en el
anexo M.
Realizando una explicación más en detalle, se distingue, en primer lugar, el uso de uno de
los botones integrados en la tarjeta Cora Z7 para realizar el control de los valores de salida
para el PWM. Aquı́ es importante destacar la necesidad de utilizar valores entre el 6 % y
12 % del duty-cycle total de la señal logrando una señal con tiempo en alto de entre 1 [ms] y
2 [ms], como se indica en el datasheet oficial de los ESC utilizados para los valores de throttle

mı́nimo y máximo respectivamente [40], los cuales, si bien no son los presentados en la coti-
zación, corresponden a una versión alternativa que cumple con los requerimientos necesarios
para el funcionamiento de los motores. En el fragmento de código presentado a continuación
se puede apreciar de mejor modo cómo se realiza el cambio entre los valores mı́nimo y máxi-
mo de PWM mediante la lectura del botón, cambiando el valor de la variable cont entre 0 y 1.
1 switch(cont){
2 case 0:
3 i = min;
4 break;
5 case 1:
6 i = max;
7 break;
8 }
Y, posteriormente, se entrega la misma salida para las 4 señales de PWM mediante la función
“Xil Out32”, como se observa en el fragmento de código presentado a continuación.
1 Xil_Out32(MY_PWM, Full_Val*(i/100.0));
2 Xil_Out32((MY_PWM+4), Full_Val*(i/100.0));
Mediante este Código, además, es posible realizar la configuración de los ESC siguiendo los
pasos descritos en la documentación disponible [40], en donde se indica el utilizar el valor
máximo del duty-cycle previo a realizar la alimentación de poder a los ESC, y posterior a una
alerta entregada mediante sonido generado por el movimiento de los motores, se debe cambiar
al valor mı́nimo del PWM. Con dichos pasos es posible asegurar la definición adecuada de
los lı́mites en la señal de control de giro de los motores, lo que permitirá, posteriormente,
realizar de forma adecuada el control de estos para el posicionamiento de la aeronave.
4.3.4. ROS
ROS corresponde a un entorno de trabajo o conjunto de herramientas orientadas al
desarrollo de sistemas robóticos robustos y complejos, permitiendo además simplificar el
trabajo colaborativo gracias a su forma de operación [41].
La forma de trabajo de ROS se basa en la definición de 2 conceptos principales, los cuales
son publicar y suscribir mientras que dichos conceptos son enlazados mediante los elementos
de tópico y mensaje, pero ¿cómo se realiza aquel enlace?
El centro de la comunicación de ROS se corresponde con la idea de tópico, el cual consiste
en un tema o identificación de la información que se desea publicar en forma de mensaje
(el cual tiene una estructura, datos y formato claramente definidos). Es ası́ que, existiendo
un tópico definido existe la posibilidad de realizar la comunicación utilizando nodos que
puedan publicar y suscribirse a él. Es decir, permite que, una vez se parametriza la forma de
comunicar la información mediante el tipo de mensaje, existan n nodos que publiquen en el

tópico, entregando información (por ejemplo, de sensores) y por otro lado m nodos suscritos
al tópico recibiendo dicha información constantemente para ejecutar alguna acción.
Además de lo anteriormente descrito, existe en ROS el concepto de package o paquete, el
cual corresponde a una pseudo-biblioteca que permite realizar determinadas tareas utilizando
nodos previamente definidos creando una red de publicaciones y suscripciones, simplificando
de este modo la interacción entre los diversos elementos que componen al robot. Un ejemplo
de esto se presenta en el diagrama de la figura Fig. 4.24, en donde se encuentran 3 tipos
de nodos, por un lado, está el nodo 3, el cual solo se encarga de publicar sobre el mensaje
1, por otro se encuentra el nodo 4, el cual solo suscribe sobre el mensaje 1 y finalmente se
encuentran los nodos 1 y 2 que se encargan de publicar y suscribir, en el caso del nodo 1 se
publica sobre el mensaje 1 y se suscribe el mensaje 2; en cambio el nodo 2 realiza el proceso
inverso.
/mensaje2
/nodo1 /mensaje1 /nodo2
/nodo3 /nodo4
Fig. 4.24: Diagrama de relaciones en comunicación ROS
Es ası́ que, para el desarrollo del presente proyecto, debido a que se utiliza Ubuntu en su
versión 20.04 [42], la versión de ROS corresponde a su versión de nombre noetic.
En esta versión, y considerando la relevancia para el presente proyecto, se destaca la
existencia de 2 packages principales a utilizar relacionados con la lectura y procesamiento
de información de sensores. Por un lado, se encuentra el de nombre “robot pose ekf”, que
corresponde a la implementación de un filtro Kalman extendido [43]. El que se encarga de
realizar las conversiones necesarias para integrar las señales obtenidas de distintos sensores
en una única salida indicando tanto posición como orientación en el espacio [44]. Por otro
lado, se encuentra el package de nombre “gps common”, que como se verá más adelante,
consiste en un nexo necesario para compartir la información de las variables sensadas por el
gps siendo convertidas de modo que sean compatibles con el package del filtro Kalman [45].
En cuanto al paquete de filtro Kalman, suscribe tópicos de 3 tipos, siendo estos de nombre
textbfimu data, textbfodom y vo, de las cuales la primera corresponde a un elemento con el
tipo de mensaje “sensor msgs/Imu” [46], mientras que las ultimas corresponden a variables
con el tipo de mensaje “nav msgs/Odometry” [?]. De estas 3 suscripciones, las relevantes para
el presente sistema radican en imu data y vo, siendo los tópicos orientados a la transferencia
de información para sensor de tipo IMU y para odometrı́a visual respectivamente, aunque
este último puede ser modificado (al ser una posición en 3 dimensiones) para recibir la
información en conjunto de GPS y barómetro.

Respecto del tópico imu data, analizándolo en más detalle, se puede indicar que está
compuesto de 3 subtópicos y 3 matrices de covarianza de 3x3 asociadas a cada subtópico. Es-
tos subtópicos contienen mensajes con los tipos que corresponden a “geometry msgs/Quaternion”
el cual consiste en el vector de rotación expresado en forma de cuaterniones y 2 subtópicos
con mensajes de tipo “geometry msgs/Vector3” correspondientes a las velocidades angulares
en [rad/s] y las aceleraciones lineales en [m/s2 ].
En cuanto al tópico vo se tiene que contiene 2 subtópicos, por un lado, se tiene uno con
mensaje de tipo “geometry msgs/PoseWithCovariance” que corresponde al almacenamiento
de la posición junto a su matriz de covarianza asociada; este punto corresponde a un men-
saje de tipo “geometry msgs/Pose”, el cual almacena la posición mediante un punto en el
espacio con un mensaje de tipo “geometry msgs/Point” y su orientación en forma de vector
de rotación utilizando cuaterniones. El otro subtópico corresponde a un mensaje de tipo
“geometry msgs/TwistWithCovariance”, que almacena la velocidad junto a su covarianza
asociada; esta velocidad es un mensaje de tipo “geometry msgs/Twist” que contiene las ve-
locidades lineales y angulares en forma de 2 vectores de 3 elementos cuyo tipo corresponde
al presentado anteriormente.
En cuanto a la salida del package, lo que corresponde, en otras palabras, al tópico pu-
blicado, consiste en “geometry msgs/PoseWithCovarianceStamped [47]”, el cual contiene la
información en un subtópico con un mensaje de tipo “geometry msgs/PoseWithCovariance”
el cual ha sido descrito con anterioridad. La importancia de este tópico radica en que, en
base a la integración de distintas variables obtenidas de los diversos sensores, se resume
la posición y orientación del robot (en este caso la aeronave) en un punto en el espacio y
un vector de rotación, los cuales pueden ser utilizados de forma directa en la estrategia de
control que se desee implementar.
Ası́, realizando el análisis del otro package (gps common), se tiene el tópico de nombre fix
como la entrada de éste. Éste tópico, contiene un mensaje de tipo “sensor msgs/NavSatFix”
[48], cuyos subtópicos más relevantes consisten en los de nombre latitude, longitude y
altitude, los cuales consisten en mensajes de tipo float de 64 bits, en conjunto con la matriz
de covarianza asociada a la posición.
Por otro lado, la salida de este package corresponde al tópico de nombre odom el cual
consiste en un mensaje de tipo “nav msgs/Odometry” ya detallado en su uso con el paquete
de filtro Kalman.
Una vez detallados los paquetes orientados al manejo de la información de sensores, es
importante detallar un package adicional de nombre “rosserial server”, el cual permite la
creación de un servidor de enlace para la comunicación entre múltiples dispositivos compati-
bles con este paquete (como lo son Arduino y ESP32 gracias a la existencia de la biblioteca
rosserial arduino). El servidor de enlace puede comunicarse mediante puerto serial utilizando
conexión vı́a cable Universal Serial Bus (USB) o utilizando un puerto glstcp mediante una
conexión a una red WiFi permitiendo una comunicación de tipo inalámbrica.
En cuanto a la implementación de lo anteriormente descrito, se debe señalar la importan-
cia de un tipo de archivo inherente al método de trabajo consistente en ROS, que corresponde
a la extensión “.launch”. Estos archivos corresponden a un texto que contiene (en forma de
código) las instrucciones de los nodos a ejecutar indicando además parámetros relevantes
como lo son la tasa de publicación y el enlace entre los nombres de tópicos publicados y
recibidos. Por ello se tiene en primer lugar el archivo “socket.launch” mostrado a continua-
ción, el cual pertenece al package “rosserial server” e indica los parámetros necesarios para

inicializar el servidor ROS.
1 <la unch>
2 <node pkg=” r o s s e r i a l s e r v e r ” type=” s o c k e t n o d e ” name=”
r o s s e r i a l s e r v e r ” />
3 <node pkg=” r o s s e r i a l p y t h o n ” type=” m e s s a g e i n f o s e r v i c e . py”
name=” r o s s e r i a l m e s s a g e i n f o ” />
4 </ l aunch>
Por otro lado, se tiene el archivo “robot pose ekf.launch”, el que, si bien en un comien-
zo inicializa únicamente el nodo “robot pose ekf” correspondiente al filtro Kalman, como
se ve a continuación, se hace la inclusión del nodo “gps conv” perteneciente al paquete
“gps common” que, como se señaló anteriormente, es necesario para interpretar los valores
obtenidos desde el GPS y transformarlos al tipo requerido por el filtro Kalman. Aquı́ se
puede ver además la definición de frecuencia de publicación del filtro Kalman, los tópicos de
entrada a utilizar para dicho filtro y la redefinición de los tópicos de entrada y salida para
ambos nodos permitiendo la interconexión de estos de forma adecuada.
1 <la unch>
2
3 <node pkg=” r o b o t p o s e e k f ” type=” r o b o t p o s e e k f ” name=”

r o b o t p o s e e k f ”>
4 <param name=” o u t p u t f r a m e ” value=” odom combined ”/>
5 <param name=” b a s e f o o t p r i n t f r a m e ” value=” b a s e f o o t p r i n t ”/>
6 <param name=” f r e q ” value=” 3 0 . 0 ”/>
7 <param name=” s e n s o r t i m e o u t ” value=” 1 . 0 ”/>
8 <param name=” odom used ” value=” f a l s e ”/>
9 <param name=” imu used ” value=” t r u e ”/>
10 <param name=” v o u s e d ” value=” t r u e ”/>
11
12 <remap from=” imu data ” t o=”/ s e n s o r m s g s /Imu” />

13 </ node>
14
15 <node name=” g p s c o n v ” pkg=”gps common” type=” utm odometry node ”

>
16 <remap from=”odom” t o=” vo ”/>
17 <remap from=” f i x ” t o=”/ s e n s o r m s g s / f i x ” />
18 <param name=” r o t c o v a r i a n c e ” value=” 99999 ” />
19 <param name=” f r a m e i d ” value=” b a s e f o o t p r i n t ” />
20 </ node>
21
22 </ l au nch>
Ası́, una vez se encuentra definido el servidor y nodos ROS a ejecutar en el computador
base, es requerido realizar el código para la tarjeta ESP32 correspondiente a la inicialización
de ROS y la conexión a la red WiFi de la placa. Para ello, se realiza la inclusión de las bi-

bliotecas presentadas a continuación, correspondientes al uso de la antena WiFi de la placa

y las bibliotecas para el uso de cada tipo de mensaje de ros a utilizar.
1 #include <WiFi.h>
2 #include <ros.h>
3 #include <ros/time.h>
4 #include <std_msgs/String.h>
5 #include <sensor_msgs/Imu.h>
6 #include <sensor_msgs/NavSatFix.h>
7 #include <geometry_msgs/PoseWithCovarianceStamped.h>
Por ello, a continuación, se requiere definir los parámetros de la conexión WiFi a utilizar,
siendo estos la SSID y su contraseña. Posteriormente, se definen, como se ve en el código a
continuación, se realiza la definición del handler de los nodos, seguido de la definición de las
publicaciones y suscripciones realizadas por el dispositivo. aquı́ cabe destacar, además, que,
al realizar la suscripción de los mensajes, se realiza la conversión de los cuaterniones, a la
forma Roll, Pitch y Yaw.
1 ros::NodeHandle nh;
2 //Publisher
3 sensor_msgs::Imu imu_msg;
4 ros::Publisher imupub("sensor_msgs/Imu", &imu_msg);
5 sensor_msgs::NavSatFix gps_msg;
6 ros::Publisher gpspub("sensor_msgs/fix", &gps_msg);
7 //Subscriber
8 void messageCb( const geometry_msgs::
PoseWithCovarianceStamped& msg){
9 float qw, qx, qy, qz;
10 float sinr_cosp, cosr_cosp, sinp, siny_cosp, cosy_cosp;
11 TrueZ = msg.pose.pose.position.z;
12 qw = msg.pose.pose.orientation.w;
13 qx = msg.pose.pose.orientation.x;
14 qy = msg.pose.pose.orientation.y;
15 qz = msg.pose.pose.orientation.z;
16 Serial.print("qx es: ");
17 Serial.println(qx);
18 Serial.print("qy es: ");
19 Serial.println(qy);
20 Serial.print("qz es: ");
21 Serial.println(qz);
22 Serial.print("qw es: ");
23 Serial.println(qw);
24 sinr_cosp = 2 * (qw * qx + qy * qz);
25 cosr_cosp = 1 - 2 * (qx * qx + qy * qy);
26 TrueRoll = atan2(sinr_cosp, cosr_cosp);

27 sinp = 2 * (qw * qy - qz * qx);

28 if(abs(sinp) >=1){
29 TruePitch = copysign(HALF_PI, sinp);
30 }
31 else{
32 TruePitch = asin(sinp);
33 }
34 siny_cosp = 2 * (qw * qz + qx * qy);
35 cosy_cosp = 1 - 2 * (qy * qy + qz * qz);
36 TrueYaw = atan2(siny_cosp, cosy_cosp);
37 Serial.print("yaw es: ");
38 Serial.println(TrueYaw);
39 }
40 ros::Subscriber<geometry_msgs::PoseWithCovarianceStamped> sub
("robot_pose_ekf/odom_combined", messageCb );
El siguiente paso corresponde a, dentro de la función setup del IDE Arduino, realizar la
inicialización de la comunicación WiFi, en conjunto con la inicialización de las publicaciones
y suscripciones de ROS, como se muestra a continuación.
1 // Conecta la ESP32 a la red WiFi

2 WiFi.begin(ssid, password);
3 while (WiFi.status() != WL_CONNECTED) {
4 delay(500);
5 Serial.print(".");
6 }
7
8 // Define la conexión al socket de rosserial

9 nh.getHardware()->setConnection(server, serverPort);
10 nh.initNode();
11
12 // Se definen publicaciones y suscripciones

13 nh.advertise(imupub);
14 nh.advertise(gpspub);
15 nh.advertise(pub2);
16 nh.subscribe(sub);
Se debe resaltar que en el caso de la suscripción se ejecuta mediante una interrupción

activada en cada publicación en el tópico, mientras que la publicación se hace de forma
manual dentro de la función loop, como se ve en el siguiente fragmento de código, en donde
se muestra de forma referencial los principales mensajes a publicar, siendo estos el header
que contiene la secuencia y tiempo de publicación, ası́ como el id del dispositivo. Posterior-
mente se publica los mensajes de información de sensores y finalmente, se utiliza la función
spinOnce para iterar en la publicación de los tópicos.
1 if (nh.connected()) {

2 Serial.println("Connected");
3 imu_msg.header.seq = seqcount;
4 imu_msg.header.frame_id = "base_footprint";
5 imu_msg.header.stamp = nh.now();
6 imu_msg.orientation.x = RetQuatX;
7 imu_msg.orientation.y = RetQuatY;
8 imu_msg.orientation.z = RetQuatZ;
9 imu_msg.orientation.w = RetQuatR;
10 imu_msg.orientation_covariance[0] = 1;
19 Serial.println("Published");
20 } else {
21 Serial.println("Not Connected");
22 }
23 nh.spinOnce();
Al realizar la inicialización de todos los módulos de ROS necesarios es posible ejecutar el

comando “rqt graph” en el terminal de ubuntu para realizar un gráfico que permite identificar
las relaciones de publicación/suscripción de los diferentes nodos (identificados con elipses),
ası́ como los diferentes tópicos involucrados (indicados con un rectángulo). En el anexo N
(simplificado en la figura 4.25) se puede observar como el nodo “rosserial server” publica
los tópicos “/sensor msgs/fix” y “/sensor msgs/Imu” correspondientes a la información de
los sensores que a su vez son suscritos por los nodos que se describieron con anterioridad
realizando un lazo cerrado mediante el tópico “/robot pose ekf/odom combined” publicado
por el nodo “robot pose ekf”. Además, el servidor ROS (en este caso mediante la tarjeta
ESP32) realiza la publicación sobre el tópico “/chatter”, el cual corresponde a un string
con el mensaje “holamundo” que permite realizar el debug sobre la comunicación previa
ejecución del algoritmo de vuelo del prototipo.

/chatter /rosserial_server /robot_pose_ekf/odom_combined
/sensor_msgs/fix /sensor_msgs/Imu
/gps_conv
/robot_pose_ekf
/vo
Fig. 4.25: Diagrama de relaciones simplificado en comunicación ROS para el presente proyecto
4.3.5. Algoritmo de control

Como se señaló en capı́tulos previos, la estrategia de control que se utilizará para co-
mandar el prototipo consiste en un control de tipo PID, por ello es necesario tener ciertas
consideraciones para realizar su implementación en código. En primer lugar, se tiene, como
se ve en la ecuación (4.1), la representación del control de tipo PID en el plano del tiempo.
Posteriormente, como se presenta en la ecuación (4.2), se tiene, gracias al uso de la transfor-
mada de Laplace, su representación en el plano de mismo nombre, lo que permite facilitar
su discretización, la cual es estrictamente necesaria para ser implementada en base a código.
En ambas ecuaciones se presenta el uso de la letra U para representar la señal de salida del
control y la letra E para indicar el error de la variable sensada respecto de la referencia,
mientras que los parámetros Kp , Ki y Kd corresponde a las ganancias proporcional, integral
y derivativa respectivamente. Por otro lado Ti corresponde al tiempo de integración.
1 t
Z
de(t)
u(t) = Kp + Ki e(τ ) dτ + Kd (4.1)
Ti 0 dt
Ki
U (s) = [Kp + + sKd ]E(s) (4.2)
s
Previo a realizar la discretización, se puede realizar un ajuste a la ecuación en el plano
de La Place añadiendo un filtro a la componente derivativa para reducir el ruido en altas
frecuencias [49], obteniendo la ecuación (4.3), en donde τ corresponde a la constante de
tiempo del filtro implementado.
Ki 1
U (s) = [Kp + + sKd ]E(s) (4.3)
s sτ + 1
Ası́, mediante la expresión de discretización por Tustin (presentada en la ecuación (4.4))
se puede realizar el cambio de la expresión del control desde el plano de La Place al plano Z
como se muestra en la ecuación (4.5). En donde T representa el tiempo de muestreo utilizado
en la discretización.
2 z−1
s≈ (4.4)
T z+1

Ki T z + 1 2Kd z − 1 1
U (z) = [Kp + ( )( )+( )( )( 2 z−1 )]E(z) (4.5)
2 z−1 T z + 1 ( T z+1 )τ + 1
Finalmente, realizando el álgebra correspondiente y realizando la transformada Z inversa,
se puede obtener la ecuación de diferencias presentada a continuación en las expresiones (4.7),
(4.8), (4.9) y (4.6).
u(k) = p(k) + i(k) + d(k) (4.6)
p(k) = Kp e(k) (4.7)
Ki T
i(k) = (e(k) + e(k − 1)) + i(k − 1) (4.8)
2
2Kd 2τ − T
d(k) = (e(k) − e(k − 1)) + d(k − 1) (4.9)
2τ + T 2τ + T
En base a esto, se puede definir un algoritmo en C/C++ que permita implementar la
estrategia de control en la tarjeta Cora Z7, no obstante, en [49], se encuentra el repositorio
con la biblioteca del control para ser implementada directamente. Ası́, a continuación se
detallarán los puntos más importantes de la biblioteca respecto a su uso. En primera ins-
tancia cabe destacar que esta biblioteca incluye además un algoritmo de tipo Wind-up que
permite evitar la saturación de la señal de control causada por la parte integral, mejorando
su desempeño.
Para simplificar la implementación se define un tipo de estructura, la cual permitirá te-
ner más de un control PID existente a la vez y que almacenará de forma independiente los
parámetros y variables necesarios para su aplicación. Ésta estructura se puede observar en
el listing presentado a continuación.
1 typedef struct{
2 //Constantes de PID
3 float Kp;
4 float Ki;
5 float Kd;
6 //Constante de tiempo para filtro derivativo
7 float tau;
8 //Limites de salida
9 float limMin;
10 float limMax;
11 //Limites de integrador para anti Wind-up
12 float limMinInt;
13 float limMaxInt;
14 //Tiempo de muestreo en segundos
15 float T;
16 //Variables de almacenamiento del controlador
17 float integrator;

18 float prevError;
19 float differentiator;
20 float prevMeasurement;
21 //Salida del controlador
22 float out;
23 } PIDController;
Por otro lado, se definen 2 funciones cuya aplicación consiste en realizar la inicialización
y actualización del controlador en cada muestreo. En cuanto a la función de inicialización
realiza directamente la limpieza de la memoria del control, mientras que la función de actua-
lización, mostrada a continuación, aplica directamente las ecuaciones vistas con anterioridad.
1 float PIDController_Update(PIDController *pid, float setpoint

, float measurement){
2 //Señal de error
3 float error = setpoint - measurement;
4 //Componente proporcional
5 float proportional = pid->Kp * error;
6 //Componente integral
7 pid->integrator = pid->integrator + 0.5f * pid->Ki *
pid->T * (error + pid->prevError);
8 //Anti wind-up
9 if(pid->integrator > pid->limMaxInt){
10 pid->integrator = pid->limMaxInt;
11 }
12 else if(pid->integrator < pid->limMinInt){
13 pid->integrator = pid->limMinInt;
14 }
15 //Componente derivativa filtrada
16 pid->differentiator = -(2.0f * pid->Kd * (measurement
- pid->prevMeasurement)+
17 (2.0f * pid->tau - pid->T) * pid->differentiator)
18 / (2.0f * pid->tau + pid->T);
19 //Se calcula la salida aplicando lı́mites
20 pid->out = proportional + pid->integrator + pid->
differentiator;
21 if(pid->out > pid->limMax){
22 pid->out = pid->limMax;
23 }
24 else if(pid->out < pid->limMin){
25 pid->out = pid->limMin;
26 }
27 //Se almacena error y medida para próxima iteración
28 pid->prevError = error;
29 pid->prevMeasurement = measurement;
30 //Se devuelve la salida del control

31 return pid->out;
32 }
4.3.6. Comunicación entre tarjetas

Para realizar la comunicación entre las tarjetas Cora Z7 y ESP32, si bien ambas pre-
sentan una amplia variedad de métodos de comunicación estándar, se debe buscar, dadas
sus caracterı́sticas puntuales, una forma de comunicarse que sea totalmente compatible con
ambas. Ası́, mediante investigación y ensayos realizados con los métodos más comunes de
comunicación, como lo son SPI y I2C y presentando la necesidad de tener a la tarjeta ESP32
como maestro del bus, es que se decide utilizar comunicación de tipo SPI. Esto, principal-
mente es debido a que en el caso de la comunicación I2C se presentan problemas en la tarjeta
Cora Z7 cuando se comporta como esclavo en el bus.
Una vez definido el tipo de comunicación, es requerido realizar la definición de una trama
de comunicación que permita transferir la información de un modo ordenado y efectivo, per-
mitiendo llevar una secuencialidad adecuada, pero sin afectar en mayor medida a los tiempos
de espera del sistema. Por ello, se define en primera instancia una trama correspondiente
a 5 bytes en la cual el primer byte a enviar corresponde al identificador de la secuencia en
la que se encuentra el maestro y que se busca igualar con el esclavo. Los 4 siguientes bytes
corresponden a la información que se desea transferir y que siempre consiste en una variable
de tipo float (es decir posee 32 bits). Dicha información, para poder ser enviada, es reque-
rido usar las 2 expresiones descritas a continuación. En el caso de la primera expresión, su
objetivo corresponde a realizar la conversión del número de punto flotante, a un vector de
4 elementos que permitan almacenar el signo, mantisa y exponente de este. Mientras que la
segunda expresión realiza el proceso inverso, realizando la conversión desde el vector hacia
el número de punto flotante.
1 //Se define la variable a codificar y su contenedor post

decodificación
2 float var2encode, vardecoded;
3 //Se define el vector de almacenamiento
4 int buff2encode[4];
5 //Se le asigna un valor a la variable a codificar, por
ejemplo, una aproximación de pi
6 var = 3.1415;
7 //Se realiza la codificación de la variable float en el
vector de 4 elementos enteros
8 *(float *)(buff2encode) = var2encode;
9 //Posterior a la comunicación se realiza el paso de
decodificación
10 vardecoded = *(float *)(buff2encode);
Otro punto por tener en consideración corresponde a la diferencia en la forma que se

realiza la comunicación SPI entre ambas tarjetas, teniendo en cuenta que, dado que las
bibliotecas vienen programadas por defecto por los fabricantes de cada placa, sus funciones

y método de transferencia de datos puede diferir en algunos aspectos. Es ası́ que, a través de
un análisis de la trama de datos, se identifican 2 incompatibilidades en la transferencia de
datos en dirección desde la tarjeta Cora Z7 hacia la tarjeta ESP32. Estas incompatibilidades
corresponden, por un lado, a la duplicación del primer dato del buffer, problema identificado
como inherente al uso de la tarjeta Cora Z7 como esclavo SPI; por otro, se produce un retraso
en la captura de datos por parte de la tarjeta ESP32 perdiendo el primer bit de la trama,
debido a la forma de realizar la lectura de datos, la cual no es compatible al 100 % con el
envı́o desde la tarjeta SoC. Como solución al problema, se extiende el buffer de comunicación
de 5 a 6 bytes, en donde el ultimo byte se deja sin información, puesto que será reemplazado
por el byte numero 5 después del desplazamiento originado por la duplicidad del primero. Por
otro lado, mediante operaciones de bitwise, se realiza el desplazamiento de los bits requeridos
para reordenar la transmisión de información, logrando ası́ reconstruir el mensaje enviado.
Esto último se realiza mediante el fragmento de código presentado a continuación, extraı́do
del código principal de la placa ESP32. Lo cual consiste en un ciclo que desplaza en 7 bytes
la comunicación, eliminando el dato duplicado y a la vez corrigiendo el desplazamiento de
bits producto del bit no leido.
1 for(int i=0; i<tamano-1;i++){
2 bufferout2[i] = (bufferout[i+1]>>1) | ((bufferout[i]&0x01
)<<7);
3 }
Finalmente, una vez que la comunicación funciona de forma bidireccional, en ambas

tarjetas se realiza un selector de casos mediante la función switch case, permitiendo de este
modo garantizar la sincronı́a del sistema con la lectura del primer byte correspondiente al
identificador de secuencia. Es aquı́ donde se define los primeros 11 pasos correspondientes al
envı́o de la información leı́da de los sensores, para a continuación tener 1 paso de publicación
de tópicos en ROS, seguido de 4 pasos para devolver la información suscrita del filtro Kalman
implementado en ROS. Cabe destacar que en el caso de la situación default del algoritmo se
realiza el almacenamiento de las lecturas de sensores a enviar para evitar mezclar lecturas
obtenidas en tiempos distintos. También es, en este paso, el instante en el cual se realiza el
reinicio del byte de secuencia de la comunicación.

5. Resultados
En el presente capı́tulo se realiza una revisión, de forma secuencial, sobre cada uno de
los ensayos experimentales realizados de forma posterior al montaje completo del prototipo.
5.1. Revisión del funcionamiento del sistema integrado

En primer lugar, se realizó una serie de pruebas sin instalar las hélices del prototipo,
con el objetivo de observar el comportamiento del sistema al integrar ROS y realizando el
movimiento de forma manual, levantando el prototipo alrededor de 1.5 [m] posterior a la
definición de la altura base. Para ello se utilizó el código presentado en el anexo Ñ, en donde,
utilizando el monitor serial conectando la tarjeta Cora Z7 vı́a USB, se puede realizar un
análisis sobre las variables internas y las publicaciones/suscripciones realizadas en los nodos
ROS. Un punto que destacar es la realización de un almacenamiento de los primeros 10
valores de altura leı́dos para que, mediante el valor promedio de estos, se pueda estimar la
altura del punto de inicio del vuelo del prototipo. Esto se ve con claridad en el fragmento de
código presentado a continuación.
1 case 0x0D:
2 for (int m=0; m<sizeof(float);m++){
3 ReadAuxB[m] = ReadBuffer[1+m];
4 }
5 TrueZ = *(float *)(ReadAuxB);
6 printf("Real = %lf\n\r",TrueZ);
7 if(setpnt == false){
8 if(TrueZ == 0){
9 initcount = initcount;
10 acumZ = 0;
11 }
12 else{
13 acumZ = acumZ + TrueZ;
14 initcount = initcount + 1;
15 }
16 if(initcount >= 10){
17 //Se calcula el promedio de las
alturas obtenidas
18 baseZ = acumZ/((initcount - 1) * 1.0)
;
19 printf("base = %lf\n\r",baseZ);
20 giroinit = true;
21 setpnt = true;
22 }
23 }
24 break;
Por otro lado, para su funcionamiento, es requerido realizar la ejecución de los archivos
“.launch” de ROS y a su vez mediante el comando rostopic echo, monitorear la información

suscrita y publicada por el filtro Kalman, permitiendo realizar una comparación sobre su
comportamiento. La plantilla de ejecución se puede ver en la figura Fig. 5.1, en donde se
tiene a la izquierda, de arriba hacia abajo, la ejecución de los “.launch” correspondientes a al
servidor ROS y filtro Kalman respectivamente. En la ventana central, por otro lado, se realiza
el monitoreo del tópico “/sensor msgs/fix”, correspondiente al tópico suscrito por el filtro
Kalman y que contiene los valores sensados de altura, con una matriz de covarianza identidad.
Ası́, en el costado derecho se monitorea el tópico “/robot pose ekf/odom combined”, lo que
corresponde al tópico publicado por el filtro Kalman.
Fig. 5.1: Plantilla de terminal de Ubuntu para monitoreo de comunicación ROS
Por ello, en primera instancia, se puede comparar el resultado obtenido mediante el

monitoreo del puerto serial de la tarjeta Cora Z7 en el instante de inicio de la ejecución del
programa respecto del momento en el que se realiza la estimación de la altura base, lo cual
se presenta en las figuras Fig. 5.2 y 5.3. Aquı́ se puede observar, en primer lugar, que luego
de iniciar el programa la salida T correspondiente a la referencia de los motores, presenta
un valor oscilante en las primeras lecturas, la cual posteriormente se normaliza en el valor
50000.039062 correspondiente al valor mı́nimo para el PWM de control y que, posterior a la
definición de la altura base, la función de PWM comienza a hacerla oscilar para ajustar la
rotación de motores y realizar el control de altitud. Aquı́ se puede ver, además, la diferencia
en torno a la décima respecto de la última altitud leı́da comparada con la altitud definida
base siendo esta última, como se señaló con anterioridad, el promedio de las lecturas previas.

Fig. 5.2: Lectura puerto serial Tarjeta Cora Z7 al inicio de la ejecución del código de prueba.

Fig. 5.3: Lectura puerto serial Tarjeta Cora Z7 al estimar la altura base.
Al realizar la comparación de los datos obtenidos para los tópicos involucrados en la

comunicación ROS, se tiene lo presentado en las figuras Fig. 5.4 y 5.5. En donde, en un
comienzo, se puede ver que los valores de la altura (Z) entre el tópico “/sensor msgs/fix”
y “/robot pose ekf/odom combined” corresponden al mismo. No obstante, una vez que se
acumulan más lecturas, se puede ver como difieren en hasta alrededor de 7 décimas, esto
último debido a que el filtro Kalman considera lecturas del barómetro y las compara con
las lecturas de la IMU corrigiendo los efectos del ruido en las lecturas del barómetro. Cabe
destacar que las comparaciones se realizan en el mismo instante en el marcador de tiempo
secs. Otro punto por tener en cuenta respecto a las lecturas del sistema ROS corresponde a
que la tasa de publicación por parte del nodo de filtro Kalman oscila entre 1 y 2 segundos,
siendo un punto de interés para futuras etapas. En cuanto a la posición en ejes X e Y se
presentan valores arbitrarios, debido a que para este desarrollo no se consideró las lecturas
del GPS.

Fig. 5.4: Lecturas del terminal Ubuntu para tópicos ROS en instantes iniciales de la prueba experimental.
Fig. 5.5: Lecturas del terminal Ubuntu para tópicos ROS en instantes finales de la prueba experimental.
5.2. Primera prueba de vuelo

Una vez realizadas las pruebas de comunicación con ROS, se procedió a realizar una
primera prueba de vuelo para identificar el comportamiento del UAV en condiciones de
trabajo normales. Para ello, se posicionó el UAV en un espacio abierto sin cables de la
red eléctrica, en este caso una cancha pública sin gente por temas de seguridad. Además,

mediante el uso de sedal de pesca de 0.35[mm] (con una resistencia para 10[kg] según su
empaque) se ató a los brazos del prototipo para tener un ancla de 5[m] para evitar que el
prototipo supere dicha altura.
Al realizar esta prueba, una vez que se inició el prototipo, se esperó a que el prototipo
comience con la secuencia de corrección de altitud, obteniendo un resultado registrado en
video, del cual fue posible extraer las capturas mostradas en la figura Fig. 5.6. Aquı́ se aprecia
como, apenas comienza a elevarse, el prototipo se inclina ampliamente hacia el costado
derecho causando su inestabilidad y posterior caı́da.
00:00:18;15 00:00:19;00
Fig. 5.6: Capturas del resultado de la primera prueba de vuelo del prototipo.
Al realizar un análisis de la situación, se comprende que, si bien se intentó dejar toda

la masa del prototipo lo más cercana a su centro, existe un desplazamiento de su centro de
gravedad, originando el efecto mostrado.
5.3. Control con compensación manual

Debido al problema presentado en la sección previa, se evaluó posibles alternativas de
solución para la compensación del problema existente. Por un lado, se encuentra la alternativa
de utilizar la orientación del prototipo entregada por el nodo ROS de filtro Kalman, la cual
se encuentra en forma de cuaterniones, los cuales mediante las ecuaciones (5.1), (5.2) y (5.3),
se pueden convertir en los ángulos Roll, Pitch y Yaw, en donde qw representa la parte real de
los cuaterniones, mientras que qx, qy y qz representan las rotaciones en torno a los ángulos
respectivos.

2(qw ∗ qx + qy ∗ qz)
φ = arctan( ) (5.1)
1 − 2(qx2 + qy 2 )
θ = arcsin(2(qw ∗ qy − qz ∗ qx)) (5.2)
2(qw ∗ qz + qx ∗ qy)
ψ = arctan( ) (5.3)
1 − 2(qy 2 + qz 2
Por otro lado, se presenta la opción de realizar una compensación de forma directa sobre
el giro de motores, buscando el valor de PWM (que en código corresponde a un valor de entre
50000 y 100000) permita reducir el efecto de desbalance causado por el centro de gravedad
del UAV.
En base a lo desarrollado, en el presente capı́tulo, se considera que el hecho del retardo que
se produce por el nodo ROS, se considera inviable realizar el control únicamente utilizando
el MMA programado. Es ası́, que se realiza una serie de pruebas de forma empı́rica en una
zona sin peatones y cubierta de pasto para reducir el impacto generado por posibles caı́das.
Además, como se presenta en la figura Fig. 5.7, se utilizó un trozo de madera previamente
nivelado para asegurar que el rotor de los 4 motores apunte directamente en dirección del
eje Z.
Fig. 5.7: Muestra del montaje utilizado para las pruebas con compensación directa sobre el giro de motores.
Para realizar este desarrollo se utilizó el código presentado en el anexo O, en el cual

mediante una interrupción se controla el giro de los motores en intervalos de 3 segundos,

realizando en primer lugar giro a bajas revoluciones para sacar los motores de inercia, a
continuación se aplica el giro de motores con un thrust base de aproximadamente un 60 % del
máximo posible, siendo modificado utilizando las variables de compensación (siendo comp,
la variable de ajuste del ángulo Roll y comp2 para ajustar el ángulo Pitch) sumándolas y
restándolas según cada motor, como se observa en el fragmento de código a continuación.
1 if (InterruptCounter > 3 && InterruptCounter <= 6){
2 TH1 = (T + comp - comp2);
3 if (TH1 < minThrottle){
4 TH1 = minThrottle;
5 }
6 if (TH1 > maxThrottle-28000){
7 TH1 = maxThrottle-28000;
8 }
9 TH2 = T - comp - comp2;
12 }
15 }
16 TH3 = T - comp + comp2;
19 }
22 }
23 TH4 = (T + comp + comp2);
26 }
29 }
30 }
Posteriormente se disminuye el giro para todos los motores y permitir el descenso del
prototipo, para finalmente dejar los motores con el valor mı́nimo de Thrust, es decir com-
pletamente detenidos.
Al realizar las pruebas con un valor de compensación de 750 para corregir el Roll y 0 en
el caso del Pitch, se observa el resultado mostrado en las capturas presentadas en la figura
Fig. 5.8, en donde se observa que dicho valor sigue sin ser suficiente para evitar la caı́da del
prototipo.

00:00:07;29 00:00:08;12
Fig. 5.8: Capturas del resultado con compensación de valor 750 para corrección de Roll y 0 para corrección
de Pitch.
Posteriormente, luego de una serie de iteraciones se llegó, como mejor aproximación, un

valor de 4650 en el caso del ajuste de Roll y 1300 en el caso del Pitch, logrando los resultados
observados en las capturas de la figura Fig. 5.9. Aquı́, se observa cómo la compensación
permite reducir la inclinación del prototipo, no obstante, debido al vuelo a baja altura para
minimizar daños por impacto, se observa la caı́da del UAV al topar el suelo con un soporte
de aterrizaje.

00:00:06;10 00:00:07;03 00:00:07;18

Fig. 5.9: Capturas del resultado con compensación de valor 4650 para corrección de Roll y 1300 para
corrección de Pitch.
Dentro de las situaciones que se pueden detallar a momento de realizar los ensayos corres-
pondientes al presente subcapı́tulo, es importante mencionar la alta influencia que presentan
factores externos en su ejecución, en particular, con la influencia del clima que, al reque-
rir realizar la experimentación en un lugar externo, no se puede asegurar en su totalidad
la igualdad de condiciones entre cada ensayo, desencadenando la necesidad de realizar una
mayor cantidad de iteraciones y con ello la rotura o flexión de las hélices, disminuyendo la
posibilidad de perfeccionar de mejor modo el despegue del prototipo.

6. Conclusiones y trabajo futuro

6.1. Conclusiones
Con respecto a las conclusiones es importante resaltar los puntos detallados a continua-
ción:
Considerando la recopilación bibliográfica es importante destacar la importancia de

las tareas de inspección para mantenimiento en la industria y, en paralelo, resaltar la
versatilidad que poseen los dispositivos UAV en términos de adaptación a los diversos
ambientes en los que se pueden utilizar. Concluyendo ası́ la factibilidad teórica que
posee el uso de dicha tecnologı́a en las tareas que requieran capacidades inalcanzables
en términos de movilidad para las personas encargadas de ello.
En cuanto a la simulación, es importante destacar la viabilidad del control que se

establece en forma teórica obteniendo una respuesta acorde a lo esperado, con un error
del 1 % en estado estacionario para la variable de altura y de alrededor de un 10 %
en el caso del control de posición (debido a la necesidad de datos empı́ricos para su
sintonización).
En cuanto al uso de ROS es importante destacar que, al permitir realizar la fusión de

sensores, aproxima de un mejor modo la posición del prototipo, haciéndolo resistente
a los efectos de ruido en las lecturas del sensor barométrico, minimizándolos en torno
al 1 %.
Como se observa en el monitoreo de variables de ROS se identifica un retardo de entre 1

a 2 segundos entre publicaciones por parte del nodo de filtro Kalman, lo cual, realizando
mayor indagación, se observa que el tiempo de demora coincide con la publicación del
nodo servidor, es decir, la demora se asocia al tiempo de lectura y transferencia de
información de sensores. Esto, por la necesidad de utilizar dos tarjetas independientes
para las funciones de procesamiento y conectividad con el maestro ROS (en este caso un
computador con Ubuntu) mediante la placa Cora Z7 y ESP32 respectivamente. Debido
a su magnitud, este delay se considera mucho mayor a la dinámica del sistema (300[ms]
en promedio) por lo que se puede indicar que corresponde al mayor problema presente
en la actualidad, sobre todo en el momento del despegue del prototipo. Por lo tanto, es
imperante la necesidad de evaluar posibles alternativas que permitan solucionar dicha
dificultad, sugiriendo revisar la viabilidad de instalar alguna distribución de Linux en
la SoC compatible con ROS y los packages presentados en el presente documento.
En cuanto a los resultados del primer ensayo de vuelo, debido a la inclinación que
inestabiliza al prototipo, se puede concluir que el centro de masa de este debe estar
desplazado a causa del montaje de los componentes electrónicos como sensores, ESC
y baterı́a. Resultando en la imposibilidad de realizar un despegue de forma exitosa en
esta experiencia experimental.
A pesar del punto anterior, en cuanto a las capacidades de diseño del prototipo, se
puede señalar que la selección de componentes a utilizar no presenta problemas desde
el punto de vista de la actuación, siendo los motores lo suficientemente potentes para

levantar el prototipo con su masa de alrededor de 700 [g], como se observa en las
capturas previas a la caı́da del UAV.
Pese a que se le entrega la misma señal PWM a los motores no se logra realizar un
despegue exitoso debido a la inclinación del prototipo en el ángulo de Roll, por ello
se hizo necesario realizar, mediante la modificación del duty cycle para cada ESC, una
compensación en las velocidades de estos corrigiendo, de este modo, de forma parcial
dicha inestabilidad.
En torno al desarrollo en general del proyecto y, como se señalo desde un comienzo,

la situacion de contingencia actual impactó de forma considerable en su desarrollo,
considerando diversos ámbitos como lo son el tiempo de espera para la entrega de
materiales, o la disponibilidad de implementos y herramientas para el trabajo. No
obstante, la mayor dificultad corresponde a la falta de un entorno controlado para
permitir realizar de mejor modo los ensayos experimentales, ya que debido a esto, el
tiempo de ensayos se vio ampliamente acotado. Entre las condiciones más relevantes
en este punto se considera:
• Condiciones climáticas adversas debido a la necesidad de realizar los ensayos en

el exterior, en particular dias lluviosos.
• Presencia de viento fuerte al momento de realizar los ensayos (adicionalmente al
punto anterior)
• Necesidad de realizar los ensayos en espacios públicos, a falta de un espacio propio,
lo que radica finalmente en la imposibilidad de adaptar una malla o medidas de
contingencia para las caidas del prototipo, originando principalmente la rotura de
un juego de hélices como consecuencia.
• Adicionalmente al punto anterior, se considera la inevitable presencia de peatones
o personas realizando deporte, lo cual implica un riesgo obligando a detener las
pruebas de vuelo para prevenir accidentes.
Es ası́ que, en resumen, se hace posible detallar como conclusión general el cumplimiento
de alrededor de un 80 % de los objetivos especı́ficos, permitiendo realizar un modelo teórico
y diseño general del prototipo de forma adecuada para las necesidades del proyecto. No
obstante, debido a situaciones no consideradas de forma previa es que no se logró implementar
en su totalidad un sistema que permitiera realizar de forma exitosa el control de altitud.
Sin perjuicio de lo anterior, se puede concluir que el presente proyecto permite sentar
las bases para un desarrollo futuro en el uso de la tecnologı́a de drones en el campo del
mantenimiento preventivo realizando tareas de inspección.
6.2. Trabajo futuro

En cuanto al trabajo futuro es importante resaltar lo siguiente:
Se presenta la necesidad de reevaluar el montaje de la electrónica del prototipo, bus-
cando alternativas que permitan localizar su centro de gravedad lo más cercano a su
centro geométrico para entregar una mayor estabilidad de forma natural, reduciendo
ası́ el impacto que tiene este efecto sobre el control.

Se debe realizar un análisis de posibles soluciones a los retardos existentes en el sis-

tema, ya sea modificando el control propuesto por alguno más robusto a costa de su
complejidad o, por otro lado, evaluar la alternativa de implementar un sistema ROS
directamente en el controlador de vuelo del prototipo evitando ası́ la necesidad de un
módulo que permita la comunicación inalámbrica vı́a WiFi.
Una vez se resuelvan los puntos anteriores, se plantea la tarea de extrapolar el control
sobre altitud a la posición sobre los otros ejes, permitiendo ası́ instalar la cámara para
realizar las rutas de inspección cumpliendo ası́ con el enfoque inicial de la presente
memoria.

Referencias
[1] Inspectioneering, “Visual inspection.” https://inspectioneering.com/tag/
visual+inspection. Accedido 25-07-2021.
[2] Pipemasters, “Visual inspection: Know the advantages and disadvantages.” http://
pipemasters.pt/blog/en/industry/visual-inspection/. Accedido 25-07-
2021.
[3] I. R. de Ingenieria del Mantenimiento, “Técnicas de mantenimiento predictivo en
plantas industriales.” http://www.renovetec.com/irim/131-tecnicas-de-
mantenimiento-predictivo. Accedido 08-12-2020.
[4] J. B. Durán, Gestión de Mantenimiento bajo estándares Internacionales como PAS 55
Asset Management.
[5] V. autores, “Industrial maintenance.” https://www.onupkeep.com/learning/
maintenance-applications/industrial-maintenance. Accedido 25-07-
2020.
[6] D. Chesworth, “Industry 4.0 techniques as a maintenance strategy (a review paper),”
01 2018.
[7] R. K. M. Lindley R. Higgins, MAINTENANCE ENGINEERING HANDBOOK. 2002.
[8] M. R. K. Smith Ricky, Industrial Machinery Repair: Best Maintenance Practices Pocket
Guide. 2003.
[9] hispadrones, “Tipos de drones.” https://www.hispadrones.com/
principiantes/aprendizaje-consejos/tipos-de-drones/, 2019. Accedi-
do 11-08-2021.
[10] M. Guillén, “Tipos de drones aéreos.” https://dronespain.pro/tipos-de-
drones-aereos/, 2020. Accedido 08-12-2020.
[11] N. Zlatanov, “Multirotor aircraft dynamics, simulation, and control,” August 2016.
[12] M. Gonzáles, “Tipos de drones: Los distintos tipos de drones que hay.” https:
//filmora.wondershare.es/drones/types-of-drones.html, 2021. Accedi-
do 11-08-2021.
[13] Ingeoexpert, “Tipos de drones: forma y método de control.”
https://ingeoexpert.com/2018/05/25/tipos-de-drones/
#Tipos de drones segun su metodo de control, 2018. Accedido 08-12-2020.
[14] A. Insights, “¿qué es la fotogrametrı́a con drones?.” https://www.aerial-
insights.co/blog/fotogrametria-con-drones/, 2018. Accedido 16-08-2021.
[15] FENERCOM, “Los drones y sus aplicaciones a la ingenierı́a civil.” https:
//www.fenercom.com/wp-content/uploads/2015/03/Los-Drones-y-
sus-Aplicaciones-a-la-Ingenieria-Civil-fenercom-2015.pdf, 2015.
Accedido 16-08-2021.

[16] K. Ogata, Modern Control Engineering. 2001.

[17] S. A. A. J. Kim and S. A. Wilkerson, “A comprehensive survey of control strategies for
autonomous quadrotors,” 05 2020.
[18] K. Åström and T. Hägglund, PID Controllers: Theory, Design and Tuning. 1995.
[19] R. Tedrake, “Underactuated robotics, chapter 8: Linear quadratic regulators.” http:
//underactuated.mit.edu/lqr.html, 2021. Accedido 08-08-2021.
[20] R. Kaur and J. Ohri, “H-infinity controller design for pneumatic servosystem: A com-
parative study,” 01 2014.
[21] A. Megretski, “Dynamics of nonlinear systems, lecture 13: Feedback linearization,” 2003.
[22] O. V. V. Mosquera, “Control backstepping de un robot scara con incertidumbre pa-
ramÉtrica,” 06 2012.
[23] B. Gallup, “Sliding mode control: A comparison of sliding surface approach dynamics.”
http://web.mit.edu/gallup/Public/project.pdf. Accedido 08-08-2021.
[24] S. H. Zak, “An introduction to model-based predictive control (mpc).” https:
//engineering.purdue.edu/˜zak/ECE680/MPC handout.pdf, 2017. Accedido
08-08-2021.
[25] J. K. M. M. M. Cirstea, A. Dinu, Neural and Fuzzy Logic Control of Drives and Power
Systems. 2002.
[26] J. K. M. M. M. Cirstea, A. Dinu, Neural and Fuzzy Logic Control of Drives and Power
Systems. 2002.
[27] A. University, “Modelling and control of quadcopter,” august 2011.
[28] E. N. F. Vaca, “Desarrollo de un modelo matemático, cinemático y dinámico con la
aplicación de software, para modificar el funcionamiento de un dron, para que este
realice monitoreo automático,” 05 2020.
[29] B. Douglas, “Drone simulation and control.” https://www.mathworks.com/
videos/drone-simulation-and-control-part-1-setting-up-the-
control-problem-1539323440930.html. Accedido 25-07-2020.
[30] R. Crowder, Electric Drives & Electromechanical Systems: Applications and Control
2nd edition. 2019.
[31] drone trest, “The complete guide to buying an fpv quadcopter frame.” https:
//blog.dronetrest.com/fpv-quadcopter-frame-buying-guide/. Accedi-
do 09-12-2020.
[32] “Grabcad.” grabcad.com. Accedido 21-9-2020.
[33] “Zmr 250 quadcopter build.” http://zmr250build.com/part-3-escs-and-
motors/. Accedido 09-12-2020.

[34] EMAX, “Emax rs2205s 2300kv 2600kv racing edition brushless motor for
fpv racing.” https://emaxmodel.com/products/emax-rs2205s-2300kv-
2600kv-racing-edition-brushless-motor-for-fpv-racing. Accedido
09-12-2020.
[35] F. Pierre, “Zmr-250 protection, landing gear, and antenna mount - * revision 3.0 *.”
https://www.thingiverse.com/thing:817622, 2015. Accedido 16-08-2021.
[36] M. Taylor, “sparkfun/sparkfun bme280 arduino library.” https://github.com/
sparkfun/SparkFun BME280 Arduino Library. Accedido 13-06-2021.
[37] N. Seidle, “sparkfun/sparkfun bno080 arduino library.” https://github.com/
sparkfun/SparkFun BNO080 Arduino Library. Accedido 13-06-2021.
[38] sparkfun, “Nmea reference manual.” https://www.sparkfun.com/datasheets/
GPS/NMEA\%20Reference\%20Manual-Rev2.1-Dec07.pdf. Accedido 15-06-
2021.
[39] M. Hart, “mikalhart/tinygpsplus.” https://github.com/mikalhart/
TinyGPSPlus. Accedido 13-06-2021.
[40] “30a bldc esc.” https://usermanual.wiki/Document/
30ABLDCESCProductManual.1338503313/pdf. Accedido 16-06-2021.
[41] “About ros.” https://www.ros.org/about-ros/. Accedido 07-05-2021.
[42] O. Robotics, “Ros noetic ninjemys.” http://wiki.ros.org/noetic, 2020. Accedi-
do 31-08-2021.
[43] A. Pascual, “Ekf y ukf: dos extensiones del filtro de kalman para sistemas no lineales
aplicadas al control de un péndulo invertido,” p. 5, 05 2006.
[44] O. Robotics, “robot pose ekf.” http://wiki.ros.org/
robot pose ekf\#robot pose ekf-1. Accedido 16-06-2021.
[45] O. Robotics, “Adding a gps sensor to the robot pose ekf filter.” http://
wiki.ros.org/robot pose\ ekf/Tutorials/AddingGpsSensor, 2011. Acce-
dido 31-08-2021.
[46] O. Robotics, “sensor msgs/imu message.” http://docs.ros.org/en/api/
sensor\ msgs/html/msg/Imu.html. Accedido 31-08-2021.
[47] O. Robotics, “geometry msgs/posewithcovariancestamped messa-
ge.” http://docs.ros.org/en/api/geometry\ msgs/html/msg/
PoseWithCovarianceStamped.html. Accedido 31-08-2021.
[48] O. Robotics, “sensor msgs/navsatfix message.” http://docs.ros.org/en/api/
sensor\ msgs/html/msg/NavSatFix.html. Accedido 31-08-2021.
[49] P. Salmony, “Pid controller implementation written in c..” https://github.com/
pms67/PID, 2020. Accedido 14-08-2021.

A.
Anexos

X
Z Y MUX
Z ref Thrust
PD
Control Posición lineal
M1 a M4 Velocidad lineal
φ ref PD Roll MMA

Comportamiento Aceleración lineal Velocidad lineal
Control
del
θ ref PD Pitch
UAV Velocidad angular Posición angular
Control
ψ ref PD
Control Yaw
φ θ ψ
MUX
Control sobre Roll, Pitch, Yaw y Thrust
Fig. A.1: Diagrama de bloques sistema con control sobre Roll, Pitch, Yaw y Thrust.
96
B.

MUX
X Y Z
Z ref Thrust
PD
Control Posición lineal
M1 a M4 Velocidad lineal
Y relativo PID φ ref PD Roll MMA

Y ref Sat.
Control Comportamiento Aceleración lineal Velocidad lineal
Control
X ref Rot ψ del
X relativo PID θ ref PD Pitch
Sat. UAV Velocidad angular Posición angular
Control Control
ψ ref PD
Control Yaw
ψ
θ
φ MUX
Control sobre posición en ejes X, Y y Z
Fig. B.1: Diagrama de bloques del sistema con control de posición en X, Y y Z.
97
C. Plano de placa de montaje para Cora Z7
Fig. C.1: Plano de placa de montaje para SoC.

D. Código ejemplo para uso de barómetro

1 /*
2 Modificaciones al ejemplo original para limitar la lectura
a
3 solo el barómetro.
4 */
5 //Biblioteca I2C
6 #include <Wire.h>
7 //Biblioteca del sensor
8 #include "SparkFunBME280.h"
9 BME280 mySensor; //Se define un objeto con la clase BME280
10
11 void setup()
12 {
13 Serial.begin(9600); //Se habilita el puerto serial
14 Serial.println("Reading basic values from BME280");
15

17 Wire.begin();
18

20 if (mySensor.beginI2C() == false)
21 {
22 Serial.println("The sensor did not respond. Please check
wiring.");
23 while(1); //Si falla se congela el programa
24 }
25 }
26
27 void loop()
28 {
29 //Se obtiene e imprime la presión en Pa
30 Serial.print(" Pressure: ");
31 Serial.print(mySensor.readFloatPressure(), 0);
32 //Se obtiene e imprime la altura en m
33 Serial.print(" Alt: ");
34 Serial.print(mySensor.readFloatAltitudeMeters(), 1);
35 Serial.println();
36
37 delay(50);
38 }

E. Código ejemplo para uso de la IMU

1 /*
2 Modificación de funcion de lecturas para
3 IMU BNO080
4 */
5 //Biblioteca I2C
6 #include <Wire.h>
7 //Biblioteca del sensor
8 #include "SparkFun_BNO080_Arduino_Library.h"
9 BNO080 myIMU;//Se define un objeto con la clase BNO080
10 void setup()
11 {
12 Serial.begin(9600); //Se habilita el puerto serial
14 Serial.println("BNO080 Read Example");
15

17 Wire.begin();
18

20 if (myIMU.begin() == false)
21 {
22 Serial.println("BNO080 not detected at default I2C
address. Check your jumpers and the hookup guide.
Freezing...");
23 while (1); //Si falla se congela el programa
24 }
25
26 Wire.setClock(400000); //Incrementa el reloj I2C a 400 kHz

27
28 myIMU.enableRotationVector(400); //Activa lecturas del

vector de rotación cada 400ms
29
30 Serial.println(F("Rotation vector enabled"));

31 Serial.println(F("Output in form i, j, k, real, accuracy"))
;
32
33 myIMU.enableGyro(400); //Activa lecturas del giroscopio

cada 400ms
34
35 Serial.println(F("Gyro enabled"));
36 Serial.println(F("Output in form x, y, z, in radians per
second"));
37

38 myIMU.enableLinearAccelerometer(400); //Activa lecturas del

acelerómetro lineal cada 400ms
39
40 Serial.println(F("Linear Accelerometer enabled"));

41 Serial.println(F("Output in form x, y, z, in m/sˆ2"));
42 }
43
44 void loop()
45 {
46 //Verifica si hay información desde la IMU
47 if (myIMU.dataAvailable() == true)
48 {
49 //De ser asi almacena los valores recibidos
50 float quatI = myIMU.getQuatI();
51 float quatJ = myIMU.getQuatJ();
52 float quatK = myIMU.getQuatK();
53 float quatReal = myIMU.getQuatReal();
54 float gx = myIMU.getGyroX();
55 float gy = myIMU.getGyroY();
56 float gz = myIMU.getGyroZ();
57 float x = myIMU.getLinAccelX();
58 float y = myIMU.getLinAccelY();
59 float z = myIMU.getLinAccelZ();
60
61 //Y los imprime por puerto serial

62 Serial.println("Vector de rotación");
63 Serial.print(quatI, 3);
64 Serial.print(" ");
65 Serial.print(quatJ, 3);
67 Serial.print(quatK, 3);
69 Serial.print(quatReal, 3);
71 Serial.println("Velocidades angulares");
73 Serial.print(gx, 2);
75 Serial.print(gy, 2);
77 Serial.print(gz, 2);
79 Serial.println("Aceleraciones lineales");
80 Serial.print(x, 2);
82 Serial.print(y, 2);

84 Serial.print(z, 2);
86 }
87 }

F. Código ejemplo para uso de sensor GPS

1 /*
2 Modificaciones al ejemplo original para realizar lecturas
al
3 GPS UBLOX NEO-6M
4 */
5 //Biblioteca para comunicación UART
6 #include <SoftwareSerial.h>
7 //Biblioteca con funciones de decodificación
8 #include <TinyGPS.h>
9 TinyGPS gps; //Se define un objeto con la clase TinyGPS
10 SoftwareSerial ss(10, 11); //Pines RX, TX para comunicación
UART
11
12 void setup()
13 {
14 Serial.begin(115200); //Se habilita puerto serial
15 ss.begin(9600); //Se inicia la comunicación UART a 9600
baudios
16
17 Serial.print("Simple TinyGPS library v. ");

18 Serial.println(TinyGPS::library_version());
19 Serial.println("by Mikal Hart");
21 }
22
23 void loop()
24 {
25 //Variable para verificar la entrada de nuevos datos
26 bool newData = false;
27 //Variables para almacenar el caracter leı́do, cantidad de
frases y fallas en la comunicación
28 unsigned long chars;
29 unsigned short sentences, failed;
30
31 // Durante 1 segundo se leen e interpretan los datos

entrantes
32 for (unsigned long start = millis(); millis() - start <
1000;)
33 {
34 //Mientras la comunicación esté disponible
35 while (ss.available())
36 {
37 //Se lee cada caracter
38 char c = ss.read();

39 if (gps.encode(c)) //Se interpreta el byte y se

verifica si entro una nueva frase
40 newData = true;
41 }
42 }
43 //Si entró una nueva frase
44 if (newData)
45 {
46 //Se definen variables para almacenar latitud y longitud
47 float flat, flon;
48 unsigned long age;
49 //Se obtiene la posición y se almacena
50 gps.f_get_position(&flat, &flon, &age);
51 //Se imprime la información obtenida
52 Serial.print("LAT=");
53 Serial.print(flat == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 :
flat, 6);
54 Serial.print(" LON=");
55 Serial.print(flon == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 :
flon, 6);
56 Serial.print(" SAT=");
57 Serial.print(gps.satellites() == TinyGPS::
GPS_INVALID_SATELLITES ? 0 : gps.satellites());
58 Serial.print(" PREC=");
59 Serial.print(gps.hdop() == TinyGPS::GPS_INVALID_HDOP ? 0
: gps.hdop());
60 }
61 //Se obtienen e imprimen las estadı́sticas de la comunicació
n
62 gps.stats(&chars, &sentences, &failed);
63 Serial.print(" CHARS=");
64 Serial.print(chars);
65 Serial.print(" SENTENCES=");
66 Serial.print(sentences);
67 Serial.print(" CSUM ERR=");
68 Serial.println(failed);
69 //Indica error si no se reciben datos del GPS por problemas
de cableado
70 if (chars == 0)
71 Serial.println("** No characters received from GPS: check
wiring **");
72 }

G. Biblioteca .h para barómetro

1 /*
2 * BME280_IIC.h
3 *
4 * Created on: 12-04-2021
5 * Author: GONZALO ROJAS
6 *
7 * Esta biblioteca es una adaptacion de la biblioteca
creada para arduino realizada por
8 * Marshall Taylor de SparkFun Electronics, la cual se
puede conseguir en el siguiente enlace
9 * https://github.com/sparkfun/BME280_Breakout. Dicha
fuente esta bajo
10 * la licencia [MIT License](http://opensource.org/
licenses/MIT).
11 * Las modificaciones realizadas son orientadas al uso
del sensor BME280, en un sistema
12 * basado en comunicación I2C utilizando las funciones
de AXI IIC disponibles para
13 * su uso en tarjetas SoC.
14 * Es importante destacar, que esta biblioteca esta
orientada al uso del módulo BME280
15 * unicamente como sensor de altitud barométrico, por lo
cual no se encuentran descritas
16 * sus funcionalidades como sensor de humedad o
temperatura.
17 *
18 * Para su correcto funcionamiento es requerido el uso
de las siguientes bibliotecas:
19 * "xiic.h"
20 * "sleep.h"
21 * "xil_printf.h"
22 *
23 *
24 */
25 #include <stdio.h> //Biblioteca para entradas y
salidas
26 #include <math.h> //Operaciones matemáticas
complejas
27 #include "xil_types.h" //Definición de tipos de xilinx
28 #include "sleep.h" //Para implementar delay
29 #include "xiic.h" //Funciones para I2C
30
31 #ifndef SRC_BME280_IIC_H_
32 #define SRC_BME280_IIC_H_

33 /**********DEFINICIONES GENERALES***************/
37 //Dirección I2C por defecto
38 #define BME_I2C_ADDRESS 0x77
39 //Modos de trabajo
40 #define BME_I2C_MODE 0
41 #define BME_MODE_SLEEP 0b00
42 #define BME_MODE_FORCED 0b01
43 #define BME_MODE_NORMAL 0b11
44 //Direcciones de registro
45 #define BME280_DIG_P1_LSB_REG 0x8E
46 #define BME280_DIG_P1_MSB_REG 0x8F
47 #define BME280_DIG_P2_LSB_REG 0x90
48 #define BME280_DIG_P2_MSB_REG 0x91
57 #define BME280_DIG_P7_LSB_REG 0x9A
58 #define BME280_DIG_P7_MSB_REG 0x9B
59 #define BME280_DIG_P8_LSB_REG 0x9C
60 #define BME280_DIG_P8_MSB_REG 0x9D
61 #define BME280_DIG_P9_LSB_REG 0x9E
62 #define BME280_DIG_P9_MSB_REG 0x9F
63 #define BME280_CHIP_ID_REG 0xD0
64 #define BME280_RST_REG 0xE0
65 #define BME280_STAT_REG 0xF3
66 #define BME280_CTRL_MEAS_REG 0xF4
67 #define BME280_CONFIG_REG 0xF5
68 #define BME280_PRESSURE_MSB_REG 0xF7
69 #define BME280_PRESSURE_LSB_REG 0xF8
70 #define BME280_PRESSURE_XLSB_REG 0xF9
71
72 /****ESTRUCTURAS DE CONFIGURACIÓN**************/
73 struct BME280_settings
74 {
75 public:
76 u8 I2CAddress;
77 u8 runMode;
78 u8 tStandby;

79 u8 filter;
80 u8 pressOverSample;
81 };
82 struct SensorCalibration
83 {
84 public:
85
86 u16 dig_P1;
87 s16 dig_P2;
88 s16 dig_P3;
89 s16 dig_P4;
90 s16 dig_P5;
91 s16 dig_P6;
92 s16 dig_P7;
93 s16 dig_P8;
94 s16 dig_P9;
95 };
96
97 /*******DEFINICIÓN CLASE DEL SENSOR*********/

98 class BME280
99 {
100 public:
101 BME280_settings settings;
102 SensorCalibration calibration;
103 s32 t_fine;
104 BME280(void); //Constructor
105 u8 begin(void); //Inicio de variables
106 bool inicio(); //Inicio del sensor
107 u8 getMode(void); //Obtiene el modo actual
108 void setMode(u8 mode); //Define el modo a usar
109 void setPressureOverSample(u8 overSampleAmount);
//Define sobremuestreo
110 void setStandbyTime(u8 timeSetting); //Define
tiempo entre muestras
111 void setFilter(u8 filterSetting); //Define el
filtro para datos
112 void setI2CAddress(u8 I2CAddress); //Modifica
dirección I2C
113 void setReferencePressure(float refPressure); //
Define presión de referencia para medición de
altitud
114 float getReferencePressure(); //Obtiene presión de
referencia actual
115 bool isMeasuring(void); //Verifica bit de medición
116 void reset(void); //Resetea el modulo, se debe
usar begin luego de esta función

117 float readFloatPressure(); //Lee presión como un

valor float
118 float readFloatAltitude(void); //Estima la altitud
como un valor float
119 void readRegisterRegion(u8*,u8,u8); //Lee una
serie de bytes, byte por byte
120 u8 readRegister(u8); //Lee solo un byte
121 s16 readRegisterInt16(u8 offset); //Almacena la
lectura de 2 bytes como un valor de 16 bit
122 void writeRegister(u8,u8); //Escribe un valor en
un determinado registro
123 private:
124 u8 checkSampleValue(u8 userValue); //Valida el
valor de sobremuestreo
125 float _referencePressure = 101325.0; //Define el
valor de presion referencial por defecto
126 };
127
128
129 #endif /* BME280_IIC_H_ */

H. Código .cc para barómetro

1 /*
2 * BME280_IIC.cc
3 *
4 * Created on: 12-04-2021
6 *
creada para arduino realizada por
8 * Marshall Taylor de SparkFun Electronics, la cual se
puede conseguir en el siguiente enlace
9 * https://github.com/sparkfun/BME280_Breakout. Dicha
fuente esta bajo
10 * la licencia [MIT License](http://opensource.org/
licenses/MIT).
del sensor BME280, en un sistema
12 * basado en comunicación I2C utilizando las funciones
de AXI IIC disponibles para
13 * su uso en tarjetas SoC.
14 * Es importante destacar, que esta biblioteca esta
orientada al uso del módulo BME280
15 * unicamente como sensor de altitud barométrico, por lo
cual no se encuentran descritas
16 * sus funcionalidades como sensor de humedad o
temperatura.
17 *
19 * "xiic.h"
20 * "sleep.h"
21 * "xil_printf.h"
22 */
23
24 #include "BME280_IIC.h"
25
26 /*************************************
27 * CONFIGURACIONES E INICIALIZACIÓN
28 *************************************/
29
30 /*
31 * Constructor--Define las configuraciones por defecto
32 */
33 BME280::BME280(void)
34 {

35 settings.I2CAddress = BME_I2C_ADDRESS;
36
37 settings.runMode = BME_MODE_NORMAL;
38 settings.tStandby = 0;
39 settings.filter = 0;
40 settings.pressOverSample = 1;
41 }
42
43 /*
44 * Realiza el inicio del sensor
45 */
46 bool BME280::inicio()
47 {
48 //settings.commInterface = BME_I2C_MODE;
49 u8 chipID = begin();
50 //printf("chip: %x \n\r",chipID); //Uncomment for
debug
51 //begin() debe retornar 0x60 o 0x58 dependiendo si el
sensor es modelo BME o BMP
52 if(chipID == 0x60){
53 return(true);
54 }
55 if(chipID == 0x58){
56 return(true);
57 }
58 return(false);
59 }
60 u8 BME280::begin()
61 {
62 //sleep(2); //Espera a que el sensor se inicie
63 u8 chipID = readRegister(BME280_CHIP_ID_REG); //Lee
el ID del sensor
64 //printf("read: %d \n \r",chipID); //Uncomment for
debug
65 if (chipID != 0x58 && chipID != 0x60){
66 return(chipID); //Si el ID no corresponde
retorna el valor obtenido
67 }
68 //Si el ID es correcto realiza la calibracion en los
registros correspondientes
69 calibration.dig_P1 =((u16)((readRegister(
BME280_DIG_P1_MSB_REG)<<8)+(readRegister(
BME280_DIG_P1_LSB_REG))));

78 //Define los valores por defecto para los parametros
de configuracion
79 setStandbyTime(settings.tStandby);
80 setFilter(settings.filter);
81 setPressureOverSample(settings.pressOverSample);
82 setMode(BME_MODE_NORMAL); //Inicia por defecto
en modo normal
83 return(readRegister(BME280_CHIP_ID_REG));
84
85 }
86
87 /*
88 * Define el modo de operacion del módulo (para más detalle
de los modos ver en datasheet).
89 * 00 = Sleep
90 * 01 and 10 = Forced
91 * 11 = Normal mode
92 */
93 void BME280::setMode(u8 mode)
94 {
95 //Si el valor pedido es mayor a 2 bit (error)
mantiene modo normal por defecto
96 if(mode>0b11){
97 mode = 0;
98 }

99 //Lee registro de información de control y lo

modifica con el modo requerido
100 u8 controlData = readRegister(BME280_CTRL_MEAS_REG);
101 controlData &=˜((1<<1)|(1<<0));
102 controlData |= mode;
103 writeRegister(BME280_CTRL_MEAS_REG, controlData);
104 }
105
106 /*
107 * Obtiene el modo actual del módulo
108 */
109 u8 BME280::getMode()
110 {
111 u8 controlData =readRegister(BME280_CTRL_MEAS_REG);
112 return(controlData & 0b00000011);
113 }
114
115 /*
116 * Define los bit de tiempo de espera dependiendo de los
requerimientos. Los modos posibles son:
117 * 0 --> 0.5ms
118 * 1 --> 62.5ms
119 * 2 --> 125ms
120 * 3 --> 250ms
121 * 4 --> 500ms
122 * 5 --> 1000ms
123 * 6 --> 10ms
124 * 7 --> 20ms
125 */
126 void BME280::setStandbyTime(u8 timeSetting)
127 {
128 if(timeSetting>0b111){
129 timeSetting = 0;
130 }
131 u8 controlData = readRegister(BME280_CONFIG_REG);
132 controlData &= ˜((1<<7)|(1<<6)|(1<<5));
133 controlData |= (timeSetting << 5);
134 writeRegister(BME280_CONFIG_REG, controlData);
135 }
136
137 /*
138 * Define los coeficientes para el filtro de respuesta a
impulso (FIR)
139 * 0 --> Filtro desactivado
140 * 1 --> coeficientes = 2


144 */
145 void BME280::setFilter(u8 filterSetting)
146 {
147 if(filterSetting>0b111){
148 filterSetting = 0;
149 }
150 u8 controlData = readRegister(BME280_CONFIG_REG);
151 controlData &= ˜((1<<4)|(1<<3)|(1<<2));
152 controlData |= (filterSetting<<2);
153 writeRegister(BME280_CONFIG_REG, controlData);
154 }
155
156 /*
157 * Define los valores de sobremuestreo 0 es desactivado, los
valores posibles
158 * son 1, 2, 4, 8 o 16
159 */
160 void BME280::setPressureOverSample(u8 overSampleAmount)
161 {
162 overSampleAmount = checkSampleValue(overSampleAmount)
;
163 u8 originalMode = getMode();
164 setMode(BME_MODE_SLEEP);
165
166 u8 controlData = readRegister(BME280_CTRL_MEAS_REG);

167 controlData &= ˜((1<<4)|(1<<3)|(1<<2));
168 controlData |= overSampleAmount << 2;
169 writeRegister(BME280_CTRL_MEAS_REG, controlData);
170
171 setMode(originalMode);
172 }
173
174 /*
175 * Valida el valor de sobremuestreo
176 */
177 u8 BME280::checkSampleValue(u8 userValue)
178 {
179 switch(userValue)
180 {
181 case(0):
182 return 0;
183 break;
184 case(1):
185 return 1;

186 break;
187 case(2):
188 return 2;
189 break;
190 case(4):
191 return 3;
192 break;
193 case(8):
194 return 4;
195 break;
196 case(16):
197 return 5;
198 break;
199 default:
200 return 1; //Si el valor
entregado no es valido por
defecto se deja en modo 1
201 break;
202 }
203 }
204
205 /*
206 * Cambia la dirección I2C para ajustarse al módulo a usar
207 */
208 void BME280::setI2CAddress(u8 address)
209 {
210 settings.I2CAddress = address;
211 }
212
213 /*
214 * Verifica si el sensor esta midiendo y retorna V o F
215 */
216 bool BME280::isMeasuring(void)
217 {
218 u8 stat = readRegister(BME280_STAT_REG);
219 return(stat&(1<<3));
220 }
221
222 /*
223 * Envia la orden de reseteo. Requiere utilizar begin
posteriormente
224 */
225 void BME280::reset(void){
226 writeRegister(BME280_RST_REG, 0xB6);
227 }
228

229 /*****************************************
230 * FUNCIONES PARA TRABAJAR CON BAROMETRO
231 *****************************************/
232
233 /*
234 * Entrega presion en Pa como un entero sin signo de 32 bit
en formato Q24.8
235 * (24 bits enteros y 8 bits fracionarios)
236 * Por ejemplo "24674867" representa 24674867/256 = 96386.2
Pa
237 */
238 float BME280::readFloatPressure(void)
239 {
240 u8 buffer[3];
241 readRegisterRegion(buffer, BME280_PRESSURE_MSB_REG,3)
;
242 s32 adc_P = ((u32)buffer[0]<<12) | ((s32)buffer
[1]<<4) |((buffer[2]>>4)&0x0F);
243 s64 var1, var2, p_acc;
244 var1 = ((s64)t_fine) - 128000;
245 var2 = var1 * var1 * (s64)calibration.dig_P6;
246 var2 = var2 + ((var1 * (s64)calibration.dig_P5)<<17);
247 var2 = var2 + (((s64)calibration.dig_P4)<<35);
248 var1 = ((var1 * var1 * (s64)calibration.dig_P3)>>8) +
((var1 * (s64)calibration.dig_P2)<<12);
249 var1 = (((((s64)1)<<47) + var1))*((s64)calibration.
dig_P1)>>33;
250 if(var1 == 0)
251 {
252 return 0; //Para evitar dividir por 0
253 }
254 p_acc = 1048576 - adc_P;
255 p_acc = (((p_acc<<31)-var2)*3125)/var1;
256 var1 = (((s64)calibration.dig_P9)*(p_acc>>13)*(p_acc
>>13))>>25;
257 var2 = (((s64)calibration.dig_P8)*p_acc)>>19;
258 p_acc = ((p_acc + var1 + var2)>>8) + (((s64)
calibration.dig_P7)<<4);
259
260 return (float)p_acc/256.0;

261 }
262
263 /*
264 * Define la presión a nivel del mar para cálculos de altitud
265 */
266 void BME280::setReferencePressure(float refPressure)

267 {
268 _referencePressure = refPressure;
269 }
270
271 /*
272 * Obtiene el valor actual de presión a nivel del mar
configurado
273 */
274 float BME280::getReferencePressure()
275 {
276 return(_referencePressure);
277 }
278
279 /*
280 * Obtiene la altitud en metros mediante la presión
utilizando la
281 * fórmula internacional barométrica.
282 */
283 float BME280::readFloatAltitude(void)
284 {
285 float heightOutput = 0;
286 heightOutput = ((float)-44330.77)*(pow(((float)
readFloatPressure()/(float)_referencePressure)
,0.190263)-(float)1);
287
288 return heightOutput;

289 }
290
291 /*****************************
292 * FUNCIONES DE UTILIDAD
293 *****************************/
294
295 /*
296 * Lee una serie de bytes, byte por byte
297 */
298 void BME280::readRegisterRegion(u8 *outputPointer, u8 offset,
u8 length)
299 {
300 //Definición de variables temporales
301 u8 i=0;
302 char c = 0;
303 u8 readBuffer[length]; //Buffer de lectura
304 u8 buffer[1]; //Buffer de escritura, para definir el
registro a leer
305 u16 StatusReg;

306 int checkpoint, check2; //Variables para obtener

información para debug
307
308 buffer[0] = offset; //Se define el registro a

leer
309

XIIC_SR_REG_OFFSET); //Se obtiene la información
del I2C
311 //Si no esta ocupado el bus
313 //Se envı́a la dirección de registro a leer
XIIC_STOP);
315 }
316 //Se solicita la lectura de los bit requeridos y se
almacenan en el buffer de lectura
317 check2 = XIic_Recv(IIC_BASE_ADDRESS, BME_I2C_ADDRESS,
readBuffer, sizeof(readBuffer), XIIC_STOP);
318
319 /*
320 * Para cada elemento del buffer de lectura se
almacena en el puntero de salida
321 * para el programa principal
322 */
323 while(i<length)
324 {
325 c = readBuffer[i];
326 *outputPointer = c;
327 outputPointer++;
328 i++;
329 }
330 }
331
332 /*
333 * Lee un byte único de un registro dado
334 */
335 u8 BME280::readRegister(u8 offset)
336 {
337 //Variables temporales y debug
338 u8 result = 30;
339 u8 numBytes = 1;
340 u8 buffer[1];
341 u16 StatusReg;
342 int checkpoint, check2;

343 //printf(" %d --- status\n\r",Status); //Descomentar

para debug
344 buffer[0] = offset;
345 //printf(" %03x --- buffer\n\r",buffer[0]); //
Descomentar para debug
346 //Se verifica disponibilidad del bus
348 //Se envia la dirección de registro a leer
XIIC_STOP);
351 }
352 //Se solicita el bit requerido
353 check2 = XIic_Recv(IIC_BASE_ADDRESS, BME_I2C_ADDRESS
,&result, numBytes, XIIC_STOP);
354 //printf("0x %x --- result\n\r",result); //
355 //printf(" %d --- check\n\r",checkpoint); //
356 //printf(" %d --- check2 success\n\r",check2); //
357
358 return result;

359 }
360
361 /*
362 * Lee 2 bytes y los almacena en una variable de 16 bits
363 */
364 s16 BME280::readRegisterInt16(u8 offset)
365 {
366 //Define un buffer de almacenamiento
367 u8 myBuffer[2];
368 //Llama a la función de lectura solicitando 2 bytes
369 readRegisterRegion(myBuffer, offset, 2);
370 //Concatena ambos bytes en una variable de 16 bit
371 s16 output = (s16)myBuffer[0] | s16(myBuffer[1]<<8);
372 return output;
373 }
374
375 /*
376 * Escribe información en un registro dado
377 */
378 void BME280::writeRegister(u8 offset, u8 dataToWrite)
379 {

380 //Se define un buffer de escritura con 2 elementos,

dirección de registro e información
381 u8 buffer[2];
382 buffer[0] = offset;
383 buffer[1] = dataToWrite;
384 u16 StatusReg;
385 //En caso de que el bus este libre se envı́an los
datos
388 XIic_Send(IIC_BASE_ADDRESS, BME_I2C_ADDRESS ,
buffer, sizeof(buffer),XIIC_STOP);
389 }
390
391 }

I. Biblioteca .h para IMU

1 /*
2 * BNO080_IIC.h
3 *
4 *Created on: 12-04-2021
6 *
creada para arduino
8 * realizada por Nathan Seide de SparkFun Electronics,
la cual se puede
9 * conseguir en el siguiente enlace
10 * https://github.com/sparkfun/
SparkFun_BNO080_Arduino_Library.
11 * Dicha fuente esta bajo la licencia GNU (General
Public License).
del módulo IMU BNO080
13 * en un sistema basado en comunicación I2C utilizando
las funciones de AXI IIC
14 * disponibles para su uso en una tarjeta SoC.
15 *
17 * "xiic.h"
18 * "sleep.h"
19 * "xil_printf.h"
20 */
21
22 #include <stdio.h> //Biblioteca para entradas y

salidas
23 #include <math.h> //Operaciones matemáticas
complejas
24 #include "xil_types.h" //Definición de tipos de xilinx
25 #include "sleep.h" //Para implementar delay
26 #include "xiic.h" //Funciones para I2C
27
28 #ifndef SRC_BNO080_IIC_H_
29 #define SRC_BNO080_IIC_H_
30 /**********DEFINICIONES GENERALES***************/
34 //Dirección I2C por defecto
35 #define BNO_ADDRESS 0x4B

36 //Limite de bytes para lectura/escritura

37 #define I2C_BUFFER_LENGTH 32
38 //Canales de comunicación disponibles
39 const u8 CHANNEL_COMMAND = 0;
40 const u8 CHANNEL_EXECUTABLE = 1;
41 const u8 CHANNEL_CONTROL = 2;
42 const u8 CHANNEL_REPORTS = 3;
43 const u8 CHANNEL_WAKE_REPORTS = 4;
44 const u8 CHANNEL_GYRO = 5;
45 //Comandos de comunicación de bajo nivel en canal 2
46 #define SHTP_REPORT_COMMAND_RESPONSE 0xF1
47 #define SHTP_REPORT_COMMAND_REQUEST 0xF2
48 #define SHTP_REPORT_FRS_READ_RESPONSE 0xF3
49 #define SHTP_REPORT_FRS_READ_REQUEST 0xF4
50 #define SHTP_REPORT_PRODUCT_ID_RESPONSE 0xF8
51 #define SHTP_REPORT_PRODUCT_ID_REQUEST 0xF9
52 #define SHTP_REPORT_BASE_TIMESTAMP 0xFB
53 #define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD
54 //ID de reportes para cada sensor
55 #define SENSOR_REPORTID_ACCELEROMETER 0x01
56 #define SENSOR_REPORTID_GYROSCOPE 0x02
57 #define SENSOR_REPORTID_MAGNETIC_FIELD 0x03
58 #define SENSOR_REPORTID_LINEAR_ACCELERATION 0x04
59 #define SENSOR_REPORTID_ROTATION_VECTOR 0x05
60 #define SENSOR_REPORTID_GRAVITY 0x06
61 #define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08
62 #define SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR 0x09
63 #define SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR 0x2A
64 #define SENSOR_REPORTID_TAP_DETECTOR 0x10
65 #define SENSOR_REPORTID_STEP_COUNTER 0x11
66 #define SENSOR_REPORTID_STABILITY_CLASSIFIER 0x13
67 #define SENSOR_REPORTID_RAW_ACCELEROMETER 0x14
68 #define SENSOR_REPORTID_RAW_GYROSCOPE 0x15
69 #define SENSOR_REPORTID_RAW_MAGNETOMETER 0x16
70 #define SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER 0x1E
71 #define SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR 0x28
72 #define SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR
0x29
73 //Para lectura de metadatos
74 #define FRS_RECORDID_ACCELEROMETER 0xE302
75 #define FRS_RECORDID_GYROSCOPE_CALIBRATED 0xE306
76 #define FRS_RECORDID_MAGNETIC_FIELD_CALIBRATED 0xE309
77 #define FRS_RECORDID_ROTATION_VECTOR 0xE30B
78 //Comandos generales
79 #define COMMAND_ERRORS 1
80 #define COMMAND_COUNTER 2

81 #define COMMAND_TARE 3
82 #define COMMAND_INITIALIZE 4
83 #define COMMAND_DCD 6
84 #define COMMAND_ME_CALIBRATE 7
85 #define COMMAND_DCD_PERIOD_SAVE 9
86 #define COMMAND_OSCILLATOR 10
87 #define COMMAND_CLEAR_DCD 11
88 //Comandos de calibración para los sensores
89 #define CALIBRATE_ACCEL 0
90 #define CALIBRATE_GYRO 1
91 #define CALIBRATE_MAG 2
92 #define CALIBRATE_PLANAR_ACCEL 3
93 #define CALIBRATE_ACCEL_GYRO_MAG 4
94 #define CALIBRATE_STOP 5
95 //Tamaño lı́mite de paquete de datos y metadatos
96 #define MAX_PACKET_SIZE 128
97 #define MAX_METADATA_SIZE 9
98
99 /*******DEFINICIÓN CLASE DEL SENSOR*********/

100 class BNO080
101 {
102 public:
103 bool begin(u8 deviceAddress); //Inicia el dispositivo
con la dirección entregada
104 void enableDebugging(); //Activa reportes de debug
105 void softReset(); //Intenta reiniciar la IMU
mediante software
106 u8 resetReason(); //Obtiene el motivo del último
reset
107 float qToFloat(s16 fixedPointValue, u8 qPoint); //
Convierte un valor Q a tipo float
108 bool receivePacket(void); //Recibe un paquete
de datos (header + datos)
109 bool getData(u16 bytesRemaining); //Dado un numero de
bytes, pide recibir los bytes en fragmentos de
largo I2C_BUFFER_LENGTH
110 bool sendPacket(u8 channelNumber, u8 dataLength);
//Escribe datos en un canal determinado
111 void printPacket(void); //Imprime por serial un
paquete (header + datos)
112 void printHeader(void); //Imprime por serial solo el
header del paquete
113 void enableRotationVector(u16 timeBetweenReports);
//Activa reportes del vector de rotación
114 void enableGameRotationVector(u16 timeBetweenReports)
; //Activa reportes del vector de rotación sin

considerar magnetómetro
115 void enableARVRStabilizedRotationVector(u16
timeBetweenReports); //Activa reportes del
vector de rotación con estabilización AR/VR
116 void enableARVRStabilizedGameRotationVector(u16
timeBetweenReports); //Activa reportes del vector
de rotación con estabilización AR/VR sin considerar
magnetómetro
117 void enableAccelerometer(u16 timeBetweenReports);
//Activa lecturas del acelerómetro
118 void enableLinearAccelerometer(u16 timeBetweenReports
); //Activa lecturas del acelerómetro en
componentes lineares
119 void enableGyro(u16 timeBetweenReports); //
Activa reportes del giroscopio
120 void enableMagnetometer(u16 timeBetweenReports);
//Activa reportes del magnetómetro
121 void enableStepCounter(u16 timeBetweenReports); //
Activa función de cuenta de pasos
122 void enableStabilityClassifier(u16 timeBetweenReports
); //Activa el clasificador de estabilidad
123 void enableActivityClassifier(u16 timeBetweenReports,
u32 activitiesToEnable, u8 (&activityConfidences)
[9]); //Activa el clasificador de actividad para
usar en conjunto con el contador de pasos
124 void enableRawAccelerometer(u16 timeBetweenReports);
//Activa la lectura del acelerometro con datos sin
procesar
125 void enableRawGyro(u16 timeBetweenReports); //Activa
la lectura del giroscopio con datos sin procesar
126 void enableRawMagnetometer(u16 timeBetweenReports);
//Activa la lectura del magnetometro con datos sin
procesar
127 void enableGyroIntegratedRotationVector(u16
timeBetweenReports); //Activa el vector de rotacion
de baja latencia mediante predicción
128
129 bool dataAvailable(void); //Verifica disponibilidad

de datos
130 void parseInputReport(void); //Analiza las lecturas
del sensor
131 void parseCommandReport(void); //Analiza las
respuestas a los comandos
132
133 /*

134 * Funciones para obtener los cuaterniones del vector

de rotación
135 */
136 float getQuatI();
137 float getQuatJ();
138 float getQuatK();
139 float getQuatReal();
140 float getQuatRadianAccuracy();
141 u8 getQuatAccuracy();
142
143 /*
144 * Funciones para obtener las aceleraciones angulares
145 */
146 float getAccelX();
147 float getAccelY();
148 float getAccelZ();
149 u8 getAccelAccuracy();
150
151 /*
152 * Funciones para obtener las aceleraciones lineales
153 */
154 float getLinAccelX();
155 float getLinAccelY();
156 float getLinAccelZ();
157 u8 getLinAccelAccuracy();
158
159 /*
160 * Funciones para obtener las componentes del
giroscopio
161 */
162 float getGyroX();
163 float getGyroY();
164 float getGyroZ();
165 u8 getGyroAccuracy();
166
167 /*
168 * Funciones para obtener las componentes con alta
tasa de refresco para el giroscopio
169 */
170 float getFastGyroX();
171 float getFastGyroY();
172 float getFastGyroZ();
173
174 /*
175 * Funciones para obtener las componentes del
magnetómetro

176 */
177 float getMagX();
178 float getMagY();
179 float getMagZ();
180 u8 getMagAccuracy();
181
182 /*
183 * Funciones de calibración
184 */
185 void calibrateAccelerometer();
186 void calibrateGyro();
187 void calibrateMagnetometer();
188 void calibratePlanarAccelerometer();
189 void calibrateAll();
190 void endCalibration();
191 void saveCalibration();
192 void requestCalibrationStatus();
193 bool calibrationComplete();
194
195 /*
196 * Funciones para obtener valores almacenados
197 */
198 u32 getTimeStamp();
199 u16 getStepCount();
200 u8 getStabilityClassifier();
201 u8 getActivityClassifier();
202 s16 getRawAccelX();
203 s16 getRawAccelY();
204 s16 getRawAccelZ();
205 s16 getRawGyroX();
206 s16 getRawGyroY();
207 s16 getRawGyroZ();
208 s16 getRawMagX();
209 s16 getRawMagY();
210 s16 getRawMagZ();
211
212 /*
213 * Convierte los valores del vector de rotacion en
componentes Roll, Pitch y Yaw
214 */
215 float getRoll();
216 float getPitch();
217 float getYaw();
218
219 /*
220 * Funciones de utilidad para enviar comandos

221 */
222 void setFeatureCommand(u8 reportID, u16
timeBetweenReports);
223 void setFeatureCommand(u8 reportID, u16
timeBetweenReports, u32 specificConfig);
224 void sendCommand(u8 command);
225 void sendCalibrateCommand(u8 thingToCalibrate);
226
227 //Funciones de metadatos

228 s16 getQ1(u16 recordID);
231 float getResolution(u16 recordID);
232 float getRange(u16 recordID);
233 u32 readFRSword(u16 recordID, u8 wordNumber);
234 void frsReadRequest(u16 recordID, u16 readOffset, u16
blockSize);
235 bool readFRSdata(u16 recordID, u8 startLocation, u8
wordsToRead);
236
237 //Variables globales

238 u8 shtpHeader[4]; //Define el header de 4 bytes
239 u8 shtpData[MAX_PACKET_SIZE]; //Buffer para los datos
leidos actualmente
240 u8 sequenceNumber[6] = {0, 0, 0, 0, 0, 0}; //Numeros
de secuencia para los 6 canales
241 u8 commandSequenceNumber = 0;
242 u32 metaData[MAX_METADATA_SIZE];
243
244 private:
245 //Variables
246 u8 _deviceAddress; //Almacena la direccion I2C
247 bool _printDebug = false; //Reportes para debug
desactivados por defecto
248
249
250 //Valores sin procesar para cada sensor

251 u16 rawAccelX, rawAccelY, rawAccelZ, accelAccuracy;
252 u16 rawLinAccelX, rawLinAccelY, rawLinAccelZ,
accelLinAccuracy;
253 u16 rawGyroX, rawGyroY, rawGyroZ, gyroAccuracy;
254 u16 rawMagX, rawMagY, rawMagZ, magAccuracy;
255 u16 rawQuatI, rawQuatJ, rawQuatK, rawQuatReal,
rawQuatRadianAccuracy, quatAccuracy;
256 u16 rawFastGyroX, rawFastGyroY, rawFastGyroZ;
257 u16 stepCount;

258 u32 timeStamp;

259 u8 stabilityClassifier;
260 u8 activityClassifier;
261 u8 *_activityConfidences;
//Arreglo para las 9
posibles actividades
262 u8 calibrationStatus;
//Byte 0 de respuesta a
calibracion
263 /*
264 * Lecturas para los sensores MEMS
265 */
266 u16 memsRawAccelX, memsRawAccelY, memsRawAccelZ;
267 u16 memsRawGyroX, memsRawGyroY, memsRawGyroZ;
268 u16 memsRawMagX, memsRawMagY, memsRawMagZ;
269
270 /*
271 * Valores Q por defecto (ver datasheet)
272 */
273 s16 rotationVector_Q1 = 14;
274 s16 rotationVectorAccuracy_Q1 = 12; //Heading
accuracy estimate in radians. The Q point is 12.
275 s16 accelerometer_Q1 = 8;
276 s16 linear_accelerometer_Q1 = 8;
277 s16 gyro_Q1 = 9;
278 s16 magnetometer_Q1 = 4;
279 s16 angular_velocity_Q1 = 10;
280 };
281
282
283
284 #endif /* SRC_BNO080_IIC_H_ */

J. Código .cc para IMU

1 /*
2 * BNO080_IIC.cc
3 *
4 *Created on: 12-04-2021
6 *
creada para arduino
8 * realizada por Nathan Seide de SparkFun Electronics,
la cual se puede
9 * conseguir en el siguiente enlace
10 * https://github.com/sparkfun/
SparkFun_BNO080_Arduino_Library.
11 * Dicha fuente esta bajo la licencia GNU (General
Public License).
del módulo IMU BNO080
13 * en un sistema basado en comunicación I2C utilizando
las funciones de AXI IIC
14 * disponibles para su uso en una tarjeta SoC.
15 *
17 * "xiic.h"
18 * "sleep.h"
19 * "xil_printf.h"
20 *
21 */
22 #include "BNO080_IIC.h"
23
24 /**************************************
25 * CONFIGURACIONES E INICIALIZACIÓN
26 **************************************/
27 /*
28 * Inicia el módulo
29 */
30 bool BNO080::begin(u8 deviceAddress)
31 {
32 _deviceAddress = deviceAddress; //Se almacena la
dirección I2C entregada
33 //printf("dev_address success"); //Descomentar para
debug
34
35 softReset(); //Reinicia la imu

36 //printf("reset done\r\n"); //Descomentar para debug

37
38 /*
39 * Verifica la comunicación con el módulo
40 */
41 shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //
Solicita el ID del módulo y su informacion de
reinicio
42 shtpData[1] = 0;
//Reservado
43
44 //Envia los 2 bytes por canal 2

45 sendPacket(CHANNEL_CONTROL, 2);
46 //Se espera por respuesta
47 if (receivePacket() == true)
48 {
49 printf(" %x",shtpData[0]);
50 if (shtpData[0] ==
SHTP_REPORT_PRODUCT_ID_RESPONSE) //Si el ID
recibido corresponde
51 {
52 if (_printDebug == true) //Si esta
activado el debug
53 {
54 /*
55 * Entrega la información
recibida
56 */
57 printf("SW Version Major: 0x %
x ",shtpData[2]);
58 printf("SW Version Minor: 0x %
x ",shtpData[3]);
59 u32 SW_Part_Number = ((u32)
shtpData[7] << 24) | ((u32)
shtpData[6] << 16) | ((u32)
shtpData[5] << 8) | ((u32)
shtpData[4]);
60 printf("SW Part Number: 0x %lx
",SW_Part_Number);
61 u32 SW_Build_Number = ((u32)
shtpData[11] << 24) | ((u32
)shtpData[10] << 16) | ((
u32)shtpData[9] << 8) | ((
u32)shtpData[8]);
62 printf("SW Build Number: 0x %
lx ",SW_Build_Number);

63 u16 SW_Version_Patch = ((u16)

shtpData[13] << 8) | ((u16)
shtpData[12]);
64 printf("SW Version Patch: 0x %
x",SW_Version_Patch);
65 }
66 return (true); //Inicio correcto
67 }
68 }
69
70 return (false); //Algo falló

71 }
72
73 /*
74 * Activa el modo debug
75 */
76 void BNO080::enableDebugging()
77 {
78 _printDebug = true;
79 }
80
81 /*
82 * Verifica si hay datos disponibles
83 */
84 bool BNO080::dataAvailable(void)
85 {
86
88 {
89 //Verifica si el paquete es un sensor
reportando datos
90 if (shtpHeader[2] == CHANNEL_REPORTS &&
shtpData[0] == SHTP_REPORT_BASE_TIMESTAMP)
91 {
92 parseInputReport(); //Dependiendo de
la variable, actualiza su
información
93 return (true);
94 }
95 //Si no son datos y es información de control
96 else if (shtpHeader[2] == CHANNEL_CONTROL)
97 {
98 parseCommandReport(); //Actualiza
respuesta a los comandos
99 return (true);
100 }

101 //Si receive packet es falso, pero existe pin de

interrupción lee los datos
102 else if(shtpHeader[2] == CHANNEL_GYRO)
103 {
104 parseInputReport(); //Dependiendo de la variable,
actualiza su información
105 return (true);
106 }
107 }
108 //Si no actualiza nada retorna falso
109 return (false);
110 }
111
112 /*
113 * Obtiene la información del report de respuesta a comandos
114 * El packete contiene:
115 * shtpHeader[0:3] --> header de 4 byte
116 * shtpData[0] --> ID de reporte
117 * shtpData[1] --> Numero de secuencia
118 * shtpData[2] --> Comando
119 * shtpData[3] --> Numero de secuencia de comando
120 * shtpData[4] --> Numero de secuencia de respuesta
121 * shtpData[5 + 0] --> R0
122 * shtpData[5 + 1] --> R1
123 * shtpData[5 + 2] --> R2
124 * shtpData[5 + 3] --> R3
125 * shtpData[5 + 4] --> R4
126 * shtpData[5 + 5] --> R5
127 * shtpData[5 + 6] --> R6
128 * shtpData[5 + 7] --> R7
129 * shtpData[5 + 8] --> R8
130 */
131 void BNO080::parseCommandReport(void)
132 {
133 if (shtpData[0] == SHTP_REPORT_COMMAND_RESPONSE)
134 {
135 //El módulo responde con esta funcion a
solicitudes de comando
136 u8 command = shtpData[2]; //Es el byte de
comando para la respuesta
137
138 if (command == COMMAND_ME_CALIBRATE) //Si el

comando es calibrar
139 {
140 calibrationStatus = shtpData[5 + 0];
//Almacena el estado de calibración

, 0=correcto, distinto de 0 =
erroneo
141 }
142 }
143 else
144 {
145 //El reporte no es soportado
146 }
147
148 }
149
150 /*
151 * Obtiene los datos del reporte de entrada, como este
reporte varia en
152 * longitud, esta duncion almacena los valores de 16 bit como
globales
153 * El paquete contiene:
154 * shtpHeader[0:3] --> Header de 4 byte
155 * shtpData[0:4] --> Marca de tiempo de 5 byte en
microsegundos desde la medición
156 * shtpData[5 + 0] --> ID de reporte (0x01 acelerometro, 0x05
vector de rotaación)
157 * shtpData[5 + 1] --> Numero de secuencia
158 * shtpData[5 + 2] --> Estado
159 * shtpData[5 + 3] --> Retraso
160 * shtpData[5 + 4:5] --> Componente i/acelerometro, x/
giroscopio, etc
161 * shtpData[5 + 6:7] --> Componente j/acelerometro, y/
giroscopio, etc
162 * shtpData[5 + 8:9] --> Componente k/acelerometro, z/
giroscopio, etc
163 * shtpData[5 + 10:11] --> Componente real/giroscopio, etc
164 * shtpData[5 + 12:13] --> Estimación de precisión
165 */
166 void BNO080::parseInputReport(void)
167 {
168 //Calcula la cantidad de bytes de dato en el paquete
169 s16 dataLength = ((u16)shtpHeader[1] << 8 |
shtpHeader[0]);
170 dataLength &= ˜(1 << 15); //Limpia el bit más
significativo. Indica que el paquete actual es
continuación del previo
171
172
173 dataLength -= 4; //Quita los bytes del header de la

cuenta de bytes

174
175 timeStamp = ((u32)shtpData[4] << (8 * 3)) | ((u32)

shtpData[3] << (8 * 2)) | ((u32)shtpData[2] << (8 *
1)) | ((u32)shtpData[1] << (8 * 0));
176
177 // Los reportes de entrada del giroscopio son

enviados por el canal gyro especial y no incluyen
la secuencia estandar
178 if(shtpHeader[2] == CHANNEL_GYRO) {
179 rawQuatI = (u16)shtpData[1] << 8 | shtpData
[0];
180 rawQuatJ = (u16)shtpData[3] << 8 | shtpData
[2];
181 rawQuatK = (u16)shtpData[5] << 8 | shtpData
[4];
182 rawQuatReal = (u16)shtpData[7] << 8 |
shtpData[6];
183 rawFastGyroX = (u16)shtpData[9] << 8 |
shtpData[8];
184 rawFastGyroY = (u16)shtpData[11] << 8 |
shtpData[10];
185 rawFastGyroZ = (u16)shtpData[13] << 8 |
shtpData[12];
186
187 return;
188 }
189
190 u8 status = shtpData[5 + 2] & 0x03; //Get status bits

191 u16 data1 = (u16)shtpData[5 + 5] << 8 | shtpData[5 +
4];
6];
8];
194 u16 data4 = 0;
195 u16 data5 = 0; //Puede ser necesario cambiar a u32
para obtener la marca de tiempo en los reportes sin
procesar
196
197 if (dataLength - 5 > 9)

198 {
199 data4 = (u16)shtpData[5 + 11] << 8 | shtpData
[5 + 10];
200 }
201 if (dataLength - 5 > 11)
202 {

203 data5 = (u16)shtpData[5 + 13] << 8 | shtpData

[5 + 12];
204 }
205
206 //Almacena los valores genéricos a su respectiva

variable global
207 if (shtpData[5] == SENSOR_REPORTID_ACCELEROMETER)
208 {
209 accelAccuracy = status;
210 rawAccelX = data1;
211 rawAccelY = data2;
212 rawAccelZ = data3;
213 }
214 else if (shtpData[5] ==
SENSOR_REPORTID_LINEAR_ACCELERATION)
215 {
216 accelLinAccuracy = status;
217 rawLinAccelX = data1;
218 rawLinAccelY = data2;
219 rawLinAccelZ = data3;
220 }
221 else if (shtpData[5] == SENSOR_REPORTID_GYROSCOPE)
222 {
223 gyroAccuracy = status;
224 rawGyroX = data1;
225 rawGyroY = data2;
226 rawGyroZ = data3;
227 }
SENSOR_REPORTID_MAGNETIC_FIELD)
229 {
230 magAccuracy = status;
231 rawMagX = data1;
232 rawMagY = data2;
233 rawMagZ = data3;
234 }
235 else if (shtpData[5] == SENSOR_REPORTID_
236 ROTATION_VECTOR ||
237 shtpData[5] == SENSOR_REPORTID_
238 GAME_ROTATION_VECTOR ||
240 AR_VR_STABILIZED_ROTATION_VECTOR ||
242 AR_VR_STABILIZED_GAME_ROTATION_VECTOR)
243 {
244 quatAccuracy = status;

245 rawQuatI = data1;

246 rawQuatJ = data2;
247 rawQuatK = data3;
248 rawQuatReal = data4;
249
250 //Solo disponible en el vector de rotacion y

estabilizado AR/VR
251 // No disponible en en game vector
252 rawQuatRadianAccuracy = data5;
253 }
254 else if (shtpData[5] == SENSOR_REPORTID_STEP_COUNTER)
255 {
256 stepCount = data3; //Bytes 8/9
257 }
SENSOR_REPORTID_STABILITY_CLASSIFIER)
259 {
260 stabilityClassifier = shtpData[5 + 4]; //Solo
byte 4
261 }
SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER)
263 {
264 activityClassifier = shtpData[5 + 5]; //
Normalmente estado
265
266 //Carga las actividades de clasificación en

el arreglo
267 for (u8 x = 0; x < 9; x++)
268 _activityConfidences[x] = shtpData[5
+ 6 + x]; //5 bytes de marca de
tiempo, el byte 6 es el de
confianza
269 }
SENSOR_REPORTID_RAW_ACCELEROMETER)
271 {
272 memsRawAccelX = data1;
273 memsRawAccelY = data2;
274 memsRawAccelZ = data3;
275 }
276 else if (shtpData[5] == SENSOR_REPORTID_RAW_GYROSCOPE
)
277 {
278 memsRawGyroX = data1;
279 memsRawGyroY = data2;

280 memsRawGyroZ = data3;

281 }
SENSOR_REPORTID_RAW_MAGNETOMETER)
283 {
284 memsRawMagX = data1;
285 memsRawMagY = data2;
286 memsRawMagZ = data3;
287 }
288 else if (shtpData[5] == SHTP_REPORT_COMMAND_RESPONSE)
289 {
290 if (_printDebug == true)
291 {
292 printf("!\r\n");
293 }
294 //El sensor responde con este reporte a las
solicitudes de comando
295 u8 command = shtpData[5 + 2]; //Byte de
comando de la respuesta
296
297 if (command == COMMAND_ME_CALIBRATE)

298 {
300 {
301 printf("ME Cal report found!"
);
302 }
303 calibrationStatus = shtpData[5 + 5];
//R0 - Status (0 = success, non-
zero = fail)
304 }
305 }
306 else
307 {
308 //No esta soportado
309 }
310 }
311
312 /************************************************
313 * CONVERSIÓN DE CUATERNIONES A ANGULOS DE EULER
314 ************************************************/
315
316 /*
317 * Entrega el Roll en radianes (rotación en torno al eje x)
318 */
319 float BNO080::getRoll()

320 {
321 float dqw = getQuatReal();
322 float dqx = getQuatI();
323 float dqy = getQuatJ();
324 float dqz = getQuatK();
325
326 float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*

dqz);
327 dqw = dqw/norm;
328 dqx = dqx/norm;
329 dqy = dqy/norm;
330 dqz = dqz/norm;
331
332 float ysqr = dqy * dqy;

333
334
335 float t0 = +2.0 * (dqw * dqx + dqy * dqz);

336 float t1 = +1.0 - 2.0 * (dqx * dqx + ysqr);
337 float roll = atan2(t0, t1);
338
339 return (roll);

340 }
341
342 /*
343 * Entrega el Pitch en radianes (rotación en torno al eje y)
344 */
345 float BNO080::getPitch()
346 {
351

dqz);
353 dqw = dqw/norm;
354 dqx = dqx/norm;
355 dqy = dqy/norm;
356 dqz = dqz/norm;
357

359
360
361 float t2 = +2.0 * (dqw * dqy - dqz * dqx);

362 t2 = t2 > 1.0 ? 1.0 : t2;
363 t2 = t2 < -1.0 ? -1.0 : t2;

364 float pitch = asin(t2);

365
366 return (pitch);

367 }
368
369 /*
370 * Entrega el Yaw en radianes (rotación en torno al eje z)
371 */
372 float BNO080::getYaw()
373 {
378

dqz);
380 dqw = dqw/norm;
381 dqx = dqx/norm;
382 dqy = dqy/norm;
383 dqz = dqz/norm;
384

386
387 // yaw (z-axis rotation)

388 float t3 = +2.0 * (dqw * dqz + dqx * dqy);
389 float t4 = +1.0 - 2.0 * (ysqr + dqz * dqz);
390 float yaw = atan2(t3, t4);
391
392 return (yaw);

393 }
394
395 /************************************************
396 * FUNCIONES DE OBTENCIÓN DE VECTOR DE ROTACIÓN
397 *************************************************/
398
399 /*
400 * Entrega el cuaternion i del vector de rotación
401 */
402 float BNO080::getQuatI()
403 {
404 float quat = qToFloat(rawQuatI, rotationVector_Q1);
405 return (quat);
406 }
407
408 /*

409 * Entrega el cuaternion j del vector de rotación

410 */
411 float BNO080::getQuatJ()
412 {
413 float quat = qToFloat(rawQuatJ, rotationVector_Q1);
414 return (quat);
415 }
416
417 /*
418 * Entrega el cuaternion k del vector de rotación
419 */
420 float BNO080::getQuatK()
421 {
422 float quat = qToFloat(rawQuatK, rotationVector_Q1);
423 return (quat);
424 }
425
426 /*
427 * Entrega el cuaternion real del vector de rotación
428 */
429 float BNO080::getQuatReal()
430 {
431 float quat = qToFloat(rawQuatReal, rotationVector_Q1)
;
432 return (quat);
433 }
434
435 /*
436 * Entrega la precisión del vector de rotación en radianes
437 */
438 float BNO080::getQuatRadianAccuracy()
439 {
440 float quat = qToFloat(rawQuatRadianAccuracy,
rotationVectorAccuracy_Q1);
441 return (quat);
442 }
443
444 /*
445 * Entrega la precision del vector de rotación
446 */
447 u8 BNO080::getQuatAccuracy()
448 {
449 return (quatAccuracy);
450 }
451
452 /************************************

453 * FUNCIONES DE ACELERACIÓN

454 ************************************/
455
456 /*
457 * Entregan componentes x, y, z de la aceleración
458 */
459 float BNO080::getAccelX()
460 {
461 float accel = qToFloat(rawAccelX, accelerometer_Q1);
462 return (accel);
463 }
464 float BNO080::getAccelY()
465 {
466 float accel = qToFloat(rawAccelY, accelerometer_Q1);
467 return (accel);
468 }
469 float BNO080::getAccelZ()
470 {
471 float accel = qToFloat(rawAccelZ, accelerometer_Q1);
472 return (accel);
473 }
474
475 /*
476 * Entrega la precisión de la aceleración
477 */
478 u8 BNO080::getAccelAccuracy()
479 {
480 return (accelAccuracy);
481 }
482
483 /*
484 * Entregan componentes x, y, z de la aceleración lineal. Por
ejemplo, compensando la gravedad
485 */
486 float BNO080::getLinAccelX()
487 {
488 float accel = qToFloat(rawLinAccelX,
linear_accelerometer_Q1);
489 return (accel);
490 }
491 float BNO080::getLinAccelY()
492 {
493 float accel = qToFloat(rawLinAccelY,
494 return (accel);
495 }

496 float BNO080::getLinAccelZ()

497 {
498 float accel = qToFloat(rawLinAccelZ,
499 return (accel);
500 }
501
502 /*
503 * Entrega la precisión de la aceleración lineal
504 */
505 u8 BNO080::getLinAccelAccuracy()
506 {
507 return (accelLinAccuracy);
508 }
509
510 /***************************************
511 * FUNCIONES DE GIROSCOPIO
512 ***************************************/
513
514 /*
515 * Entregan componentes x, y, z del giroscopio
516 */
517 float BNO080::getGyroX()
518 {
519 float gyro = qToFloat(rawGyroX, gyro_Q1);
520 return (gyro);
521 }
522 float BNO080::getGyroY()
523 {
524 float gyro = qToFloat(rawGyroY, gyro_Q1);
525 return (gyro);
526 }
527 float BNO080::getGyroZ()
528 {
529 float gyro = qToFloat(rawGyroZ, gyro_Q1);
530 return (gyro);
531 }
532
533 /*
534 * Entrega la precisión de las lecturas del giroscopio
535 */
536 u8 BNO080::getGyroAccuracy()
537 {
538 return (gyroAccuracy);
539 }
540

541 /*************************************
542 * FUNCIONES DE MAGNETÓMETRO
543 *************************************/
544
545 /*
546 * Entregan componentes x, y, z del magnetómetro
547 */
548 float BNO080::getMagX()
549 {
550 float mag = qToFloat(rawMagX, magnetometer_Q1);
551 return (mag);
552 }
553 float BNO080::getMagY()
554 {
555 float mag = qToFloat(rawMagY, magnetometer_Q1);
556 return (mag);
557 }
558 float BNO080::getMagZ()
559 {
560 float mag = qToFloat(rawMagZ, magnetometer_Q1);
561 return (mag);
562 }
563
564 /*
565 * Entrega la precisión de las lecturas del magnetómetro
566 */
567 u8 BNO080::getMagAccuracy()
568 {
569 return (magAccuracy);
570 }
571
572 /*********************************************************
573 * FUNCIONES DE GIROSCOPIO
574 * CON ALTA TASA DE ACTUALIZACIÓN
575 *********************************************************/
576 /*
577 * Entregan componentes x, y, z del giroscopio con alta tasa
de actualización
578 */
579 float BNO080::getFastGyroX()
580 {
581 float gyro = qToFloat(rawFastGyroX,
angular_velocity_Q1);
582 return (gyro);
583 }
584 float BNO080::getFastGyroY()

585 {
586 float gyro = qToFloat(rawFastGyroY,
587 return (gyro);
588 }
589 float BNO080::getFastGyroZ()
590 {
591 float gyro = qToFloat(rawFastGyroZ,
592 return (gyro);
593 }
594
595 /*******************************
596 * FUNCIONES DE CUENTAPASOS
597 *******************************/
598
599 /*
600 * Entrega la cuenta de pasos
601 */
602 u16 BNO080::getStepCount()
603 {
604 return (stepCount);
605 }
606
607 /*
608 * Entrega el clasificador de estabilidad
609 */
610 u8 BNO080::getStabilityClassifier()
611 {
612 return (stabilityClassifier);
613 }
614
615 /*
616 * Entrega el clasificador de actividad
617 */
618 u8 BNO080::getActivityClassifier()
619 {
620 return (activityClassifier);
621 }
622
623 /*
624 * Entrega la marca de tiempo
625 */
626 u32 BNO080::getTimeStamp()
627 {
628 return (timeStamp);

629 }
630
631 /*****************************************************
632 * FUNCIONES PARA DATOS SIN PROCESAR DE SENSORES MEMS
633 *****************************************************/
634
635 /*
636 * Entregan componentes x, y, z sin procesar para el
acelerometro
637 */
638 s16 BNO080::getRawAccelX()
639 {
640 return (memsRawAccelX);
641 }
642 s16 BNO080::getRawAccelY()
643 {
644 return (memsRawAccelY);
645 }
646 s16 BNO080::getRawAccelZ()
647 {
648 return (memsRawAccelZ);
649 }
650
651 /*
652 * Entregan componentes x, y, z sin porcesar para el
giroscopio
653 */
654 s16 BNO080::getRawGyroX()
655 {
656 return (memsRawGyroX);
657 }
658 s16 BNO080::getRawGyroY()
659 {
660 return (memsRawGyroY);
661 }
662 s16 BNO080::getRawGyroZ()
663 {
664 return (memsRawGyroZ);
665 }
666
667 /*
668 * Entregan componentes x, y, z sin procesar para el
magnetómetro
669 */
670 s16 BNO080::getRawMagX()
671 {

672 return (memsRawMagX);

673 }
674 s16 BNO080::getRawMagY()
675 {
676 return (memsRawMagY);
677 }
678 s16 BNO080::getRawMagZ()
679 {
680 return (memsRawMagZ);
681 }
682
683 /*************************************
684 * FUNCIONES PARA METADATOS
685 *************************************/
686
687 /*
688 * Dado un ID de registro, lee el valor Q1 de los registros
de metadatos en el FRS
689 * Q1 es usado para todos los calculos de datos de sensores
690 */
691 s16 BNO080::getQ1(u16 recordID)
692 {
693 //Q1 siempre son los 16 bits menos significativos de
la palabra 7
694 u16 q = readFRSword(recordID, 7) & 0xFFFF;
695 return (q);
696 }
697
698 /*
700 * Q2 es usado para el bias de sensor
701 */
703 {
704 //Q2 siempre son los 16 bits más significativos de la
palabra 7
705 u16 q = readFRSword(recordID, 7) >> 16;
706 return (q);
707 }
708
709 /*
711 * Q3 es usado para cambiar la sensibilidad de los sensores
712 */


714 {
715 //Q3 siempre son los 16 bit más significativos de la
palabra 8
716 u16 q = readFRSword(recordID, 8) >> 16;
717 return (q);
718 }
719
720 /*
721 * Dado un ID de registro, lee el valor de resolución del
registro de metadatos en el FRS para un sensor dado
722 */
723 float BNO080::getResolution(u16 recordID)
724 {
725 /*
726 * Es el mismo valor usado en el reporte de entrada
del sensor
727 * Deberia ser Q1
728 */
729 s16 Q = getQ1(recordID);
730
731 //La resolución es siempre la palabra 2

732 u32 value = readFRSword(recordID, 2);
733
734 float resolution = qToFloat(value, Q);

735
736 return (resolution);

737 }
738
739 /*
740 * Dado un ID de registro, lee el valor de rango del registro
de metadatos en el FRS para un sensor dado
741 */
742 float BNO080::getRange(u16 recordID)
743 {
744 /*
745 * Es el mismo valor usado en el reporte de entrada
del sensor
746 * Deberia ser Q1
747 */
748 s16 Q = getQ1(recordID);
749
750 //Siempre es la palabra 1

751 u32 value = readFRSword(recordID, 1);
752
753 float range = qToFloat(value, Q);

754
755 return (range);

756 }
757
758 /*
759 * Dado un ID de registro y un número de palabra, busca la
información de la palabra
760 * Util para obtener un valor Q, rango, etc
761 * Utiliza datos de FRS para obtener objetos multi-palabra de
un sensor
762 */
763 u32 BNO080::readFRSword(u16 recordID, u8 wordNumber)
764 {
765 if (readFRSdata(recordID, wordNumber, 1) == true) //
Obtiene el número de palabras, solo una palabra en
longitud de FRS
766 return (metaData[0]);
//Retorna esta
palabra
767
768 return (0); //Error

769 }
770
771 /*
772 * Pide al sensor información del FRS (Flash Record System)
773 */
774 void BNO080::frsReadRequest(u16 recordID, u16 readOffset, u16
blockSize)
775 {
776 shtpData[0] = SHTP_REPORT_FRS_READ_REQUEST; //
Solicitud de lectura FRS
777 shtpData[1] = 0;
//Reservado
778 shtpData[2] = (readOffset >> 0) & 0xFF; //Lee
el bit menos significativo del offset
779 shtpData[3] = (readOffset >> 8) & 0xFF; //Lee
el bit más significativo del offset
780 shtpData[4] = (recordID >> 0) & 0xFF; //Bit
menos significativo del FRS
781 shtpData[5] = (recordID >> 8) & 0xFF; //Bit
más significativo del FRS
782 shtpData[6] = (blockSize >> 0) & 0xFF; //Bit
menos significativo del tamaño de bloque
783 shtpData[7] = (blockSize >> 8) & 0xFF; //Bit
más significativo del tamaño de bloque
784

785 //Transmite el paquete de 8 bytes en canal 2

787 }
788
789 /*
790 * Dado un ID de registro y los bytes de Inicio/Parada, lee
los datos del FRS para el sensor
791 * Retorna verdadero si el arreglo de metadatos carga
exitosamente
792 * Retorna falso si falla
793 */
794 bool BNO080::readFRSdata(u16 recordID, u8 startLocation, u8
wordsToRead)
795 {
796 u8 spot = 0;
797
798 //Primero se envia una solicitud de FRS

799 frsReadRequest(recordID, startLocation, wordsToRead);
//Desde el inicio del registro lee un número de
palabras
800
801 //Lee bytes hasta que el FRS reporte lectura completa

802 while (1)
803 {
804 //Se espera respuesta
805 while (1)
806 {
807 u8 counter = 0;
808 while (receivePacket() == false)
809 {
810 if (counter++ > 100)
811 return (false);
812 usleep(1000);
813 }
814
815 /*
816 * Teniendo el paquete lo inspecciona
buscando contenidos adecuados
817 */
SHTP_REPORT_FRS_READ_RESPONSE)
819 if (((u16)shtpData[13] << 8 |
shtpData[12]) == recordID)
820 break; //Es el
paquete buscado
821 }

822
823 u8 dataLength = shtpData[1] >> 4;

824 u8 frsStatus = shtpData[1] & 0x0F;
825
826 u32 data0 = (u32)shtpData[7] << 24 | (u32)

shtpData[6] << 16 | (u32)shtpData[5] << 8 |
(u32)shtpData[4];
827 u32 data1 = (u32)shtpData[11] << 24 | (u32)
shtpData[10] << 16 | (u32)shtpData[9] << 8
| (u32)shtpData[8];
828
829 //Almacena las palabras en arreglo de

metadatos
830 if (dataLength > 0)
831 {
832 metaData[spot++] = data0;
833 }
834 if (dataLength > 1)
835 {
836 metaData[spot++] = data1;
837 }
838
839 if (spot >= MAX_METADATA_SIZE)

840 {
842 printf("metaData array over
run. Returning.");
843 return (true); //No queda espacio en
el arreglo
844 }
845
846 if (frsStatus == 3 || frsStatus == 6 ||

frsStatus == 7)
847 {
848 return (true); //El status de FRS
esta completo
849 }
850 }
851 }
852
853 /*************************************************
854 * FUNCIONES PARA REINICIO Y CONVERSION DE DATOS
855 *************************************************/
856
857 /*
858 * Envia el comando para reiniciar el módulo

859 * Lee los paquetes de información del sensor

860 * El sensor se puede reiniciar dos veces si se intenta
muchas veces muy seguido
861 */
862 void BNO080::softReset(void)
863 {
864 shtpData[0] = 1; //Comando de reinicio
865
866 //Intenta comunicarse con el sensor

867 sendPacket(CHANNEL_EXECUTABLE, 1); //Transmite los
paquetes en canal 1, un byte
868 //printPacket();
869 //printf("sendpacket success\r\n"); //Descomentar
para debug
870 //Lee los datos y los descarta
871
872 usleep(100000);
873 while (receivePacket() == true)
874 //printPacket();
875 ;
876 //printf("receive 1 done\r\n"); //Descomentar para
debug
877 usleep(100000);
878 while (receivePacket() == true)
879 ;
880 //printf("receive 2 done\r\n"); //Descomentar para
debug
881 }
882
883 /*
884 * Obtiene la razón del último reinicio
885 * 1 = PoR (Power on Reset)
886 * 2 = Reinicio interno
887 * 3 = Watchdog (reinicio por error)
888 * 4 = Reinicio externo
889 * 5 = Otro
890 */
891 u8 BNO080::resetReason()
892 {
893 shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //
Solicita ID del producto e informacion de reinicio
894 shtpData[1] = 0;
//Reservado
895
896 //Transmite el paquete por el canal 2, 2 bytes


898
899 //Se espera por respuesta

901 {
SHTP_REPORT_PRODUCT_ID_RESPONSE)
903 {
904 return (shtpData[1]);
905 }
906 }
907
908 return (0);

909 }
910
911 /*
912 * Dado un registro y un valor punto Q, se convierte a punto
float
913 */
914 float BNO080::qToFloat(s16 fixedPointValue, u8 qPoint)
915 {
916 float qFloat = fixedPointValue;
917 qFloat *= pow(2, qPoint * -1);
918 return (qFloat);
919 }
920
921 /***************************************
922 * FUNCIONES DE ACTIVACIÓN
923 ***************************************/
924
925 /*
926 * Envı́a el paquete para activar el vector de rotación
927 */
928 void BNO080::enableRotationVector(u16 timeBetweenReports)
929 {
930 setFeatureCommand(SENSOR_REPORTID_ROTATION_VECTOR,
931 }
932
933 /*
934 * Envı́a el paquete para activar el vector de rotación con
estabilización AR/VR
935 */
936 void BNO080::enableARVRStabilizedRotationVector(u16
timeBetweenReports)
937 {

938 setFeatureCommand(
SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR,
939 }
940
941 /*
942 * Envı́a el paquete para activar el vector de rotación sin
magnetómetro
943 */
944 void BNO080::enableGameRotationVector(u16 timeBetweenReports)
945 {
SENSOR_REPORTID_GAME_ROTATION_VECTOR,
947 }
948
949 /*
950 * Envı́a el paquete para activar el vector de rotación sin
magnetómetro con estabilización AR/VR
951 */
952 void BNO080::enableARVRStabilizedGameRotationVector(u16
timeBetweenReports)
953 {
954 setFeatureCommand(SENSOR_REPORTID_AR_VR_
955 STABILIZED_GAME_ROTATION_VECTOR, timeBetweenReports);
956 }
957
958 /*
959 * Envı́a el paquete para activar el acelerómetro
960 */
961 void BNO080::enableAccelerometer(u16 timeBetweenReports)
962 {
963 setFeatureCommand(SENSOR_REPORTID_ACCELEROMETER,
964 }
965
966 /*
967 * Envı́a el paquete para activar el acelerómetro lineal
968 */
969 void BNO080::enableLinearAccelerometer(u16 timeBetweenReports
)
970 {
971 setFeatureCommand(SENSOR_REPORTID_LINEAR_ACCELERATION
, timeBetweenReports);
972 }
973

974 /*
975 * Envı́a el paquete para activar el giroscopio
976 */
977 void BNO080::enableGyro(u16 timeBetweenReports)
978 {
979 setFeatureCommand(SENSOR_REPORTID_GYROSCOPE,
980 }
981
982 /*
983 * Envı́a el paquete para activar el magnetómetro
984 */
985 void BNO080::enableMagnetometer(u16 timeBetweenReports)
986 {
987 setFeatureCommand(SENSOR_REPORTID_MAGNETIC_FIELD,
988 }
989
990 /*
991 * Envı́a el paquete para activar el vector de rotación con
integración de girocopio de alta actualización
992 */
993 void BNO080::enableGyroIntegratedRotationVector(u16
timeBetweenReports)
994 {
SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR,
996 }
997
998 /*
999 * Envı́a el paquete para activar el contador de pasos
1000 */
1001 void BNO080::enableStepCounter(u16 timeBetweenReports)
1002 {
1003 setFeatureCommand(SENSOR_REPORTID_STEP_COUNTER,
1004 }
1005
1006 /*
1007 * Envı́a el paquete para activar el clasificador de
estabilidad
1008 */
1009 void BNO080::enableStabilityClassifier(u16 timeBetweenReports
)
1010 {

SENSOR_REPORTID_STABILITY_CLASSIFIER,
1012 }
1013
1014 /*
1015 * Envı́a el paquete para activar las lecturas del
acelerómetro sin procesar
1016 * Es estrictamente necesario además activar los reportes
básicos
1017 */
1018 void BNO080::enableRawAccelerometer(u16 timeBetweenReports)
1019 {
1020 setFeatureCommand(SENSOR_REPORTID_RAW_ACCELEROMETER,
1021 }
1022
1023 /*
1024 * Envı́a el paquete para activar las lecturas del giroscopio
sin procesar
básicos
1026 */
1027 void BNO080::enableRawGyro(u16 timeBetweenReports)
1028 {
1029 setFeatureCommand(SENSOR_REPORTID_RAW_GYROSCOPE,
1030 }
1031
1032 /*
1033 * Envı́a el paquete para activar las lecturas del
magnetómetro sin procesar
básicos
1035 */
1036 void BNO080::enableRawMagnetometer(u16 timeBetweenReports)
1037 {
1038 setFeatureCommand(SENSOR_REPORTID_RAW_MAGNETOMETER,
1039 }
1040
1041 /*
1042 * Envı́a el paquete para activar el clasificador de actividad
1043 */
1044 void BNO080::enableActivityClassifier(u16 timeBetweenReports,
u32 activitiesToEnable, u8 (&activityConfidences)[9])

1045 {
1046 _activityConfidences = activityConfidences; //
Almacena el puntero al arreglo
1047
SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER,
timeBetweenReports, activitiesToEnable);
1049 }
1050
1051 /*************************************
1052 * FUNCIONES DE CALIBRACIÓN
1053 *************************************/
1054
1055 /*
1056 * Envı́a los comandos para iniciar la calibración del
acelerómetro
1057 */
1058 void BNO080::calibrateAccelerometer()
1059 {
1060 sendCalibrateCommand(CALIBRATE_ACCEL);
1061 }
1062
1063 /*
giroscopio
1065 */
1066 void BNO080::calibrateGyro()
1067 {
1068 sendCalibrateCommand(CALIBRATE_GYRO);
1069 }
1070
1071 /*
magnetómetro
1073 */
1074 void BNO080::calibrateMagnetometer()
1075 {
1076 sendCalibrateCommand(CALIBRATE_MAG);
1077 }
1078
1079 /*
acelerómetro lineal/planar
1081 */
1082 void BNO080::calibratePlanarAccelerometer()
1083 {

1084 sendCalibrateCommand(CALIBRATE_PLANAR_ACCEL);
1085 }
1086
1087 /*
1088 * Envı́a los comandos para iniciar la calibración de todos
los sensores
1089 * Ver procedimiento de calibracion en documento 1000-4044
punto 2.2
1090 */
1091 void BNO080::calibrateAll()
1092 {
1093 sendCalibrateCommand(CALIBRATE_ACCEL_GYRO_MAG);
1094 }
1095
1096 /*
1097 * Termina la calibración
1098 */
1099 void BNO080::endCalibration()
1100 {
1101 sendCalibrateCommand(CALIBRATE_STOP); //Desactiva
todas las calibraciones
1102 }
1103
1104 /*
1105 * Respuesta de calibración, ver página 51 del manual de
referencias
1106 * El byte 5 es analizado en la lectura de paquete y
almacenada en el estatus de calibración
1107 */
1108 bool BNO080::calibrationComplete()
1109 {
1110 if (calibrationStatus == 0)
1111 return (true);
1112 return (false);
1113 }
1114
1115 /***************************************
1116 * FUNCIONES DE COMUNICACIÓN
1117 ***************************************/
1118
1119 /*
1120 * Dado un ID de reporte, ordena al módulo comenzar a
reportar valores
1121 */
1122 void BNO080::setFeatureCommand(u8 reportID, u16
timeBetweenReports)

1123 {
1124 setFeatureCommand(reportID, timeBetweenReports, 0);
//Sin configuración especı́fica
1125 }
1126
1127 /*
1128 * Dado un ID de reporte de un sensor, le ordena al módulo
comenzar a reportar valores
1129 * Ademas define la palabra especı́fica de configuración. Util
para el clasificador de actividades
1130 */
1131 void BNO080::setFeatureCommand(u8 reportID, u16
timeBetweenReports, u32 specificConfig)
1132 {
1133 long microsBetweenReports = (long)timeBetweenReports
* 1000L;
1134
1135 shtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //

Define el comando de caracterı́stica
1136 shtpData[1] = reportID;
//ID de reporte de
caracterı́stica. 0x01 = Acelerómetro, 0x05 = Vector
de rotación
1137 shtpData[2] = 0;
//Indicadores de
caracterı́stica
1138 shtpData[3] = 0;
//Cambio de
sensitividad (LSB)
1139 shtpData[4] = 0;
//Cambio de
sensitividad (MSB)
1140 shtpData[5] = (microsBetweenReports >> 0) & 0xFF; //
Intervalo de reporte (LSB) en microsegundos. 0
x7A120 = 500ms
Intervalo de reporte
Intervalo de reporte
Intervalo de reporte (MSB)
1144 shtpData[9] = 0;
//Intervalo de
lotes (LSB)
1145 shtpData[10] = 0;
//Intervalo de

lotes
1146 shtpData[11] = 0;
//Intervalo de
lotes
1147 shtpData[12] = 0;
//Intervalo de
lotes (MSB)
1148 shtpData[13] = (specificConfig >> 0) & 0xFF; //
Configuración especifica del sensor (LSB)
Configuración especifica del sensor
Configuración especifica del sensor
Configuración especifica del sensor (MSB)
1152
1153 //Transmit packet on channel 2, 17 bytes

1155 }
1156
1157 /*
1158 * Le ordena al sensor ejecutar un comando
1159 */
1160 void BNO080::sendCommand(u8 command)
1161 {
1162 shtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //
Solicitud de comando
1163 shtpData[1] = commandSequenceNumber++; //Incrementa
automáticamente en cada ejecución
1164 shtpData[2] = command;
//Comando
1165
1166 //La funcion solicitante debe definir lo siguiente

1167 /*shtpData[3] = 0; //P0
1168 shtpData[4] = 0; //P1
1169 shtpData[5] = 0; //P2
1170 shtpData[6] = 0;
1171 shtpData[7] = 0;
1172 shtpData[8] = 0;
1173 shtpData[9] = 0;
1174 shtpData[10] = 0;
1175 shtpData[11] = 0;*/
1176
1177 //Transmite los 12 bytes del paquete en canal 2

1179 }

1180
1181 /*
1182 * Le indica al módulo iniciar la calibración
1183 * Ver documento de calibración 1000-4044
1184 */
1185 void BNO080::sendCalibrateCommand(u8 thingToCalibrate)
1186 {
1187 /*shtpData[3] = 0; //P0 - Activación de calibración
de acelerómetro
1188 shtpData[4] = 0; //P1 - Activación de calibración de
giroscopio
1189 shtpData[5] = 0; //P2 - Activación de calibracion de
magentómetro
1190 shtpData[6] = 0; //P3 - Subcomando 0x00
1191 shtpData[7] = 0; //P4 - Activación de calibración de
acelerómetro planar
1192 shtpData[8] = 0; //P5 - Reservado
1195 shtpData[11] = 0; //P8 - Reservado*/
1196
1197 for (u8 x = 3; x < 12; x++) //Limpia esta seccion del
arreglo shtpData
1198 shtpData[x] = 0;
1199
1200 if (thingToCalibrate == CALIBRATE_ACCEL)

1201 shtpData[3] = 1;
1202 else if (thingToCalibrate == CALIBRATE_GYRO)
1203 shtpData[4] = 1;
1204 else if (thingToCalibrate == CALIBRATE_MAG)
1205 shtpData[5] = 1;
1206 else if (thingToCalibrate == CALIBRATE_PLANAR_ACCEL)
1207 shtpData[7] = 1;
1208 else if (thingToCalibrate == CALIBRATE_ACCEL_GYRO_MAG
)
1209 {
1210 shtpData[3] = 1;
1211 shtpData[4] = 1;
1212 shtpData[5] = 1;
1213 }
1214 else if (thingToCalibrate == CALIBRATE_STOP)
1215 ; //No hace nada, los bytes son seteados a
cero
1216
1217 //Hace el estado interno calStatus no cero (operacion

fallida) para que el usuario pueda testear

mientras se espera
1218 calibrationStatus = 1;
1219
1220 //Usando este paquete se envı́a el comando

1221 sendCommand(COMMAND_ME_CALIBRATE);
1222 }
1223
1224 /*
1225 * Solicita el estado de calibración desde el módulo
1226 */
1227 void BNO080::requestCalibrationStatus()
1228 {
1229 /*shtpData[3] = 0; //P0 - Reservado
1232 shtpData[6] = 0; //P3 - 0x01 - Subcomando obtener
calibración
1238
1239 for (u8 x = 3; x < 12; x++) //Limpa la sección del

arreglo shtpData
1241
1242 shtpData[6] = 0x01; //P3 - 0x01 - Subcomando: Obtener

calibración
1243
1244 //Usando el paquete envı́a el comando

1245 sendCommand(COMMAND_ME_CALIBRATE);
1246 }
1247
1248 /*
1249 * Ordena al módulo guardar los datos dinámicos de
calibración en la memoria flash
1250 */
1251 void BNO080::saveCalibration()
1252 {
1253 /*shtpData[3] = 0; //P0 - Reservado


1262
1263 for (u8 x = 3; x < 12; x++) //Limpia esta sección del
arreglo shtpData
1265
1266 //Usando el paquete, envı́a el comando

1267 sendCommand(COMMAND_DCD); //Guarda el comando DCD (
Data carrier detect)
1268 }
1269
1270 /*
1271 * Verifica si hay nuevos datos disponibles
1272 * Lee los contenidos del paquete entrante en el arreglo
shtpData
1273 */
1274 bool BNO080::receivePacket(void)
1275 {
1276
1277 int var;

1278 u8 readBuffer[4];
1279
1280 var = XIic_Recv(IIC_BASE_ADDRESS, BNO_ADDRESS,

readBuffer, sizeof(readBuffer), XIIC_STOP);
1281 //printf(" %d\r\n ",var); //Descomentar para debug
1282 //Obitiene el header (los 4 primeros bytes)
1283
1284 u8 packetLSB = readBuffer[0];

1285 u8 packetMSB = readBuffer[1];
1286 u8 channelNumber = readBuffer[2];
1287 u8 sequenceNumber = readBuffer[3];
1288
1289 //Almacena la informacion del header

1290 shtpHeader[0] = packetLSB;
1291 shtpHeader[1] = packetMSB;
1292 shtpHeader[2] = channelNumber;
1293 shtpHeader[3] = sequenceNumber;
1294 //printf(" %x\r\n %x\r\n %x\r\n\ %x\r\n",IMU.shtpHeader
[0],IMU.shtpHeader[1],IMU.shtpHeader[2],IMU.
shtpHeader[3]); //Descomentar para debug
1295 //Calcula el numero de datos del paquete
1296 s16 dataLength = ((u16)packetMSB << 8 | packetLSB);
1297 //printf(" %d\r\n", dataLength); //Descomentar para
debug

1298 dataLength &= ˜(1 << 15); //Limpia el bit más

significativo indica que la comunicación es
continuacion de algo previo
debug
1300
1301 if (dataLength == 0)
1302 {
1303 //El paquete esta vacı́o
1304 return (false); //Completado
1305 }
1306 dataLength -= 4; //Quita los bytes del header de la
cuenta
debug
1308 //printf("header got\r\n"); //Descomentar para debug
1309 getData(dataLength);
1310 //printf("data got\r\n");
1311 return (true); //Completado
1312 }
1313
1314 /*
1315 * Envı́a múltiples solicitudes al sensor hasta que todos los
bytes de datos son recibidos
1316 * El buffer shtpData tiene una capacidad máxima de
MAX_PACKET_SIZE. Los bytes extra seran perdidos.
1317 */
1318 bool BNO080::getData(u16 bytesRemaining)
1319 {
1320 u16 dataSpot = 0; //Iicia al comienzo del arreglo
shtpData
1321 int var;
1322 while(bytesRemaining > 0){
1323 u16 numberOfBytesToRead = bytesRemaining;
1324 if(numberOfBytesToRead>(I2C_BUFFER_LENGTH -
4)){
1325 numberOfBytesToRead = (
I2C_BUFFER_LENGTH - 4);
1326 }
1327 //printf("number2read %d\r\n",
numberOfBytesToRead); //Descomentar para
debug
1328 u8 readBuffer[numberOfBytesToRead + 4];
1329 var = XIic_Recv(IIC_BASE_ADDRESS, BNO_ADDRESS
,readBuffer, sizeof(readBuffer), XIIC_STOP)
;

1330 for (u8 x = 4; x < (numberOfBytesToRead+4); x

++)
1331 {
1332 if(dataSpot < MAX_PACKET_SIZE){
1333 shtpData[dataSpot++] =
readBuffer[x]; //Almacena
la informacion en el
arreglo shtpData
1334 }
1335 else{
1336 }
1337 }
1338 bytesRemaining -= numberOfBytesToRead;
1339
1340 }
1341 //Configura una serie de lecturas de trozos de 32
bytes
1342
1343 return (true); //Listo

1344 }
1345
1346 /*
1347 * Dado el paquete de datos, envı́a el header y luego los
datos
1348 * Retorna falso si el sensor no retorna ACK
1349 */
1350 bool BNO080::sendPacket(u8 channelNumber, u8 dataLength)
1351 {
1352 u8 packetLength = dataLength + 4; //Añade 4 bytes
para el header
1353 u8 buffer2send[packetLength];
1354 u16 StatusReg;
1355 int checkpoint;
1356 //if(packetLength > I2C_BUFFER_LENGTH) return(false);
//Se estan tratando de enviar muchos datos,
dividir en paquetes más pequeños
1357
1358
1359 //Almacena en el buffer de envı́o los 4 bytes del

header
1360 buffer2send[0] = (packetLength & 0xFF);
1361 buffer2send[1] = (packetLength >> 8);
1362 buffer2send[2] = channelNumber;
1363 buffer2send[3] = sequenceNumber[channelNumber]++;
1364
1365

1366 //Almacena en el buffer de envı́o los bytes de datos

1367 for (u8 i = 0; i < dataLength; i++)
1368 {
1369 buffer2send[4+i] = shtpData[i];
1370
1371 }
1372
1373 //Si el bus I2C no esta ocupado, envı́a el buffer

BNO_ADDRESS , buffer2send, sizeof(
buffer2send),XIIC_STOP);
1377 }
1378 else{
1379 return(false); //Bus ocupado
1380 }
1381
1382 return (true);

1383 }
1384
1385 /***************************************
1386 * FUNCIONES DE DEBUG
1387 ***************************************/
1388
1389 /*
1390 * Imprime los contenidos del paquete actual incluyendo
header y datos
1391 */
1392 void BNO080::printPacket(void)
1393 {
1395 {
1396 u16 packetLength = (u16)shtpHeader[1] << 8 |
shtpHeader[0];
1397
1398 //Imprime los 4 bytes del header

1399 printf("Header:");
1400 for (u8 x = 0; x < 4; x++)
1401 {
1402 printf(" ");
1403 if (shtpHeader[x] < 0x10)
1404 printf("0");
1405 printf("0x %x",shtpHeader[x]);
1406 }

1407
1408 u8 printLength = packetLength - 4;

1409 if (printLength > 40)
1410 printLength = 40; //Se define un
lı́mite artificial para evitar
impresión excesiva
1411
1412 printf(" Body:");

1413 for (u8 x = 0; x < printLength; x++)
1414 {
1415 printf(" ");
1416 if (shtpData[x] < 0x10)
1417 printf("0");
1418 printf("0x %x",shtpData[x]);
1419 }
1420
1421 if (packetLength & 1 << 15)

1422 {
1423 printf(" [Continued packet] ");
1424 packetLength &= ˜(1 << 15);
1425 }
1426
1427 printf(" Length:");

1428 printf(" %d",packetLength);
1429
1430 printf(" Channel:");

1431 if (shtpHeader[2] == 0)
1432 printf("Command");
1433 else if (shtpHeader[2] == 1)
1434 printf("Executable");
1436 printf("Control");
1438 printf("Sensor-report");
1440 printf("Wake-report");
1442 printf("Gyro-vector");
1443 else
1444 printf(" %d",shtpHeader[2]);
1445
1446 printf("\r\n");
1447 }
1448 }
1449
1450 /*

1451 * Imprime el contenido del header del paquete actual

1452 */
1453 void BNO080::printHeader(void)
1454 {
1456 {
1457 //Imprime el header
1458 printf("Header:");
1459 for (u8 x = 0; x < 4; x++)
1460 {
1461 printf(" ");
1462 if (shtpHeader[x] < 0x10)
1463 printf("0");
1464 printf("x %x",shtpHeader[x]);
1465 }
1466 printf("\r\n");
1467 }
1468 }

K. Código AXI-PWM.v
1 ‘timescale 1 ns / 1 ps
2
3 module PWM_60hz_v1_0_S00_AXI #
4 (
5 // Users to add parameters here
6 parameter integer PWM_COUNTER_MAX = 1024,
7 // User parameters ends
8 // Do not modify the parameters beyond this line
9
10 // Width of S_AXI data bus

11 parameter integer C_S_AXI_DATA_WIDTH = 32,
12 // Width of S_AXI address bus
13 parameter integer C_S_AXI_ADDR_WIDTH = 4
14 )
15 (
16 // Users to add ports here
17 output wire PWM0,
21 // User ports ends
22 // Do not modify the ports beyond this line
23
24 // Global Clock Signal

25 input wire S_AXI_ACLK,
26 // Global Reset Signal. This Signal is Active LOW
27 input wire S_AXI_ARESETN,
28 // Write address (issued by master, acceped by Slave
)
29 input wire [C_S_AXI_ADDR_WIDTH-1 : 0] S_AXI_AWADDR,
30 // Write channel Protection type. This signal
indicates the
31 // privilege and security level of the transaction,
and whether
32 // the transaction is a data access or an
instruction access.
33 input wire [2 : 0] S_AXI_AWPROT,
34 // Write address valid. This signal indicates that
the master signaling
35 // valid write address and control information.
36 input wire S_AXI_AWVALID,
37 // Write address ready. This signal indicates that
the slave is ready
38 // to accept an address and associated control
signals.
39 output wire S_AXI_AWREADY,

40 // Write data (issued by master, acceped by Slave)

41 input wire [C_S_AXI_DATA_WIDTH-1 : 0] S_AXI_WDATA,
42 // Write strobes. This signal indicates which byte
lanes hold
43 // valid data. There is one write strobe bit for
each eight
44 // bits of the write data bus.
45 input wire [(C_S_AXI_DATA_WIDTH/8)-1 : 0]
S_AXI_WSTRB,
46 // Write valid. This signal indicates that valid
write
47 // data and strobes are available.
48 input wire S_AXI_WVALID,
49 // Write ready. This signal indicates that the slave
50 // can accept the write data.
51 output wire S_AXI_WREADY,
52 // Write response. This signal indicates the status
53 // of the write transaction.
54 output wire [1 : 0] S_AXI_BRESP,
55 // Write response valid. This signal indicates that
the channel
56 // is signaling a valid write response.
57 output wire S_AXI_BVALID,
58 // Response ready. This signal indicates that the
master
59 // can accept a write response.
60 input wire S_AXI_BREADY,
61 // Read address (issued by master, acceped by Slave)
62 input wire [C_S_AXI_ADDR_WIDTH-1 : 0] S_AXI_ARADDR,
63 // Protection type. This signal indicates the
privilege
64 // and security level of the transaction, and
whether the
65 // transaction is a data access or an instruction
access.
66 input wire [2 : 0] S_AXI_ARPROT,
67 // Read address valid. This signal indicates that
the channel
68 // is signaling valid read address and control
information.
69 input wire S_AXI_ARVALID,
70 // Read address ready. This signal indicates that
the slave is
71 // ready to accept an address and associated control
signals.
72 output wire S_AXI_ARREADY,
73 // Read data (issued by slave)
74 output wire [C_S_AXI_DATA_WIDTH-1 : 0] S_AXI_RDATA,

75 // Read response. This signal indicates the status

of the
76 // read transfer.
77 output wire [1 : 0] S_AXI_RRESP,
78 // Read valid. This signal indicates that the
channel is
79 // signaling the required read data.
80 output wire S_AXI_RVALID,
81 // Read ready. This signal indicates that the master
can
82 // accept the read data and response information.
83 input wire S_AXI_RREADY
84 );
85
86 // AXI4LITE signals
87 reg [C_S_AXI_ADDR_WIDTH-1 : 0] axi_awaddr;
88 reg axi_awready;
89 reg axi_wready;
90 reg [1 : 0] axi_bresp;
91 reg axi_bvalid;
92 reg [C_S_AXI_ADDR_WIDTH-1 : 0] axi_araddr;
93 reg axi_arready;
94 reg [C_S_AXI_DATA_WIDTH-1 : 0] axi_rdata;
95 reg [1 : 0] axi_rresp;
96 reg axi_rvalid;
97
98 // Example-specific design signals

99 // local parameter for addressing 32 bit / 64 bit
C_S_AXI_DATA_WIDTH
100 // ADDR_LSB is used for addressing 32/64 bit registers/
memories
101 // ADDR_LSB = 2 for 32 bits (n downto 2)
102 // ADDR_LSB = 3 for 64 bits (n downto 3)
103 localparam integer ADDR_LSB = (C_S_AXI_DATA_WIDTH/32) + 1;
104 localparam integer OPT_MEM_ADDR_BITS = 1;
105 //----------------------------------------------
106 //-- Signals for user logic register space example
107 //------------------------------------------------
108 //-- Number of Slave Registers 4
109 reg [C_S_AXI_DATA_WIDTH-1:0] slv_reg0;
113 wire slv_reg_rden;
114 wire slv_reg_wren;
115 reg [C_S_AXI_DATA_WIDTH-1:0] reg_data_out;
116 integer byte_index;
117 reg aw_en;
118

119 // I/O Connections assignments

120
121 assign S_AXI_AWREADY = axi_awready;

122 assign S_AXI_WREADY = axi_wready;
123 assign S_AXI_BRESP = axi_bresp;
124 assign S_AXI_BVALID = axi_bvalid;
125 assign S_AXI_ARREADY = axi_arready;
126 assign S_AXI_RDATA = axi_rdata;
127 assign S_AXI_RRESP = axi_rresp;
128 assign S_AXI_RVALID = axi_rvalid;
129 // Implement axi_awready generation
130 // axi_awready is asserted for one S_AXI_ACLK clock cycle
when both
131 // S_AXI_AWVALID and S_AXI_WVALID are asserted. axi_awready
is
132 // de-asserted when reset is low.
133
134 always @( posedge S_AXI_ACLK )

135 begin
136 if ( S_AXI_ARESETN == 1’b0 )
137 begin
138 axi_awready <= 1’b0;
139 aw_en <= 1’b1;
140 end
141 else
142 begin
143 if (ãxi_awready && S_AXI_AWVALID && S_AXI_WVALID &&
aw_en)
144 begin
145 // slave is ready to accept write address when
146 // there is a valid write address and write data
147 // on the write address and data bus. This design
148 // expects no outstanding transactions.
150 aw_en <= 1’b0;
151 end
152 else if (S_AXI_BREADY && axi_bvalid)
153 begin
154 aw_en <= 1’b1;
156 end
157 else
158 begin
160 end
161 end
162 end
163
164 // Implement axi_awaddr latching

165 // This process is used to latch the address when both

166 // S_AXI_AWVALID and S_AXI_WVALID are valid.
167

169 begin
171 begin
172 axi_awaddr <= 0;
173 end
174 else
175 begin
176 if (ãxi_awready && S_AXI_AWVALID && S_AXI_WVALID &&
aw_en)
177 begin
178 // Write Address latching
179 axi_awaddr <= S_AXI_AWADDR;
180 end
181 end
182 end
183
184 // Implement axi_wready generation

185 // axi_wready is asserted for one S_AXI_ACLK clock cycle
when both
186 // S_AXI_AWVALID and S_AXI_WVALID are asserted. axi_wready
is
187 // de-asserted when reset is low.
188

190 begin
192 begin
193 axi_wready <= 1’b0;
194 end
195 else
196 begin
197 if (ãxi_wready && S_AXI_WVALID && S_AXI_AWVALID &&
aw_en )
198 begin
199 // slave is ready to accept write data when
200 // there is a valid write address and write data
201 // on the write address and data bus. This design
202 // expects no outstanding transactions.
204 end
205 else
206 begin
208 end
209 end

210 end
211
212 // Implement memory mapped register select and write logic

generation
213 // The write data is accepted and written to memory mapped
registers when
214 // axi_awready, S_AXI_WVALID, axi_wready and S_AXI_WVALID
are asserted. Write strobes are used to
215 // select byte enables of slave registers while writing.
216 // These registers are cleared when reset (active low) is
applied.
217 // Slave register write enable is asserted when valid
address and data are available
218 // and the slave is ready to accept the write address and
write data.
219 assign slv_reg_wren = axi_wready && S_AXI_WVALID &&
axi_awready && S_AXI_AWVALID;
220

222 begin
224 begin
225 slv_reg0 <= 0;
226 slv_reg1 <= 0;
227 slv_reg2 <= 0;
228 slv_reg3 <= 0;
229 end
230 else begin
231 if (slv_reg_wren)
232 begin
233 case ( axi_awaddr[ADDR_LSB+OPT_MEM_ADDR_BITS:
ADDR_LSB] )
234 2’h0:
235 for ( byte_index = 0; byte_index <= (
C_S_AXI_DATA_WIDTH/8)-1; byte_index =
byte_index+1 )
236 if ( S_AXI_WSTRB[byte_index] == 1 ) begin
237 // Respective byte enables are asserted as
per write strobes
238 // Slave register 0
239 slv_reg0[(byte_index*8) +: 8] <= S_AXI_WDATA
[(byte_index*8) +: 8];
240 end
241 2’h1:
byte_index+1 )


per write strobes
247 end
248 2’h2:
byte_index+1 )
per write strobes
254 end
255 2’h3:
byte_index+1 )
per write strobes
261 end
262 default : begin
263 slv_reg0 <= slv_reg0;
267 end
268 endcase
269 end
270 end
271 end
272
273 // Implement write response logic generation

274 // The write response and response valid signals are
asserted by the slave
275 // when axi_wready, S_AXI_WVALID, axi_wready and
S_AXI_WVALID are asserted.
276 // This marks the acceptance of address and indicates the
status of
277 // write transaction.
278

280 begin
282 begin
283 axi_bvalid <= 0;
284 axi_bresp <= 2’b0;
285 end
286 else
287 begin
288 if (axi_awready && S_AXI_AWVALID && ãxi_bvalid &&
axi_wready && S_AXI_WVALID)
289 begin
290 // indicates a valid write response is available
291 axi_bvalid <= 1’b1;
292 axi_bresp <= 2’b0; // ’OKAY’ response
293 end // work error responses in
future
294 else
295 begin
296 if (S_AXI_BREADY && axi_bvalid)
297 //check if bready is asserted while bvalid is
high)
298 //(there is a possibility that bready is always
asserted high)
299 begin
300 axi_bvalid <= 1’b0;
301 end
302 end
303 end
304 end
305
306 // Implement axi_arready generation

307 // axi_arready is asserted for one S_AXI_ACLK clock cycle
when
308 // S_AXI_ARVALID is asserted. axi_awready is
309 // de-asserted when reset (active low) is asserted.
310 // The read address is also latched when S_AXI_ARVALID is
311 // asserted. axi_araddr is reset to zero on reset assertion.
312

314 begin
316 begin
317 axi_arready <= 1’b0;
318 axi_araddr <= 32’b0;
319 end
320 else
321 begin
322 if (ãxi_arready && S_AXI_ARVALID)
323 begin

324 // indicates that the slave has acceped the valid

read address
326 // Read address latching
327 axi_araddr <= S_AXI_ARADDR;
328 end
329 else
330 begin
332 end
333 end
334 end
335
336 // Implement axi_arvalid generation

337 // axi_rvalid is asserted for one S_AXI_ACLK clock cycle
when both
338 // S_AXI_ARVALID and axi_arready are asserted. The slave
registers
339 // data are available on the axi_rdata bus at this instance.
The
340 // assertion of axi_rvalid marks the validity of read data
on the
341 // bus and axi_rresp indicates the status of read
transaction.axi_rvalid
342 // is deasserted on reset (active low). axi_rresp and
axi_rdata are
343 // cleared to zero on reset (active low).
345 begin
347 begin
348 axi_rvalid <= 0;
349 axi_rresp <= 0;
350 end
351 else
352 begin
353 if (axi_arready && S_AXI_ARVALID && ãxi_rvalid)
354 begin
355 // Valid read data is available at the read data
bus
356 axi_rvalid <= 1’b1;
357 axi_rresp <= 2’b0; // ’OKAY’ response
358 end
359 else if (axi_rvalid && S_AXI_RREADY)
360 begin
361 // Read data is accepted by the master
362 axi_rvalid <= 1’b0;
363 end
364 end

365 end
366
367 // Implement memory mapped register select and read logic

generation
368 // Slave register read enable is asserted when valid address
is available
369 // and the slave is ready to accept the read address.
370 assign slv_reg_rden = axi_arready & S_AXI_ARVALID & ˜
axi_rvalid;
371 always @(*)
372 begin
373 // Address decoding for reading registers
374 case ( axi_araddr[ADDR_LSB+OPT_MEM_ADDR_BITS:ADDR_LSB]
)
375 2’h0 : reg_data_out <= slv_reg0;
379 default : reg_data_out <= 0;
380 endcase
381 end
382
383 // Output register or memory read data

385 begin
387 begin
388 axi_rdata <= 0;
389 end
390 else
391 begin
392 // When there is a valid read address (S_AXI_ARVALID)
with
393 // acceptance of read address by the slave (
axi_arready),
394 // output the read dada
395 if (slv_reg_rden)
396 begin
397 axi_rdata <= reg_data_out; // register read
data
398 end
399 end
400 end
401
402 // Add user logic here

403 reg [19:0] counter = 0;
404
405 //simple counter

406 always @(posedge S_AXI_ACLK) begin

407 if(counter < PWM_COUNTER_MAX-1)

408 counter <= counter + 1;
409 else
410 counter <= 0;
411 end
412
413 //comparator statements that drive the PWM signal

418
419 // User logic ends

420
421 endmodule

L. Código PWM.v
1 ‘timescale 1 ns / 1 ps
2
3 module PWM_60hz_v1_0 #
4 (
5 // Users to add parameters here
6 parameter integer PWM_COUNTER_MAX = 128,
7 // User parameters ends
8 // Do not modify the parameters beyond this line
9
10
11 // Parameters of Axi Slave Bus Interface S00_AXI

12 parameter integer C_S00_AXI_DATA_WIDTH = 32,
13 parameter integer C_S00_AXI_ADDR_WIDTH = 4
14 )
15 (
16 // Users to add ports here
21 // User ports ends
22 // Do not modify the ports beyond this line
23
24
25 // Ports of Axi Slave Bus Interface S00_AXI

26 input wire s00_axi_aclk,
27 input wire s00_axi_aresetn,
28 input wire [C_S00_AXI_ADDR_WIDTH-1 : 0]
s00_axi_awaddr,
29 input wire [2 : 0] s00_axi_awprot,
30 input wire s00_axi_awvalid,
31 output wire s00_axi_awready,
32 input wire [C_S00_AXI_DATA_WIDTH-1 : 0]
s00_axi_wdata,
33 input wire [(C_S00_AXI_DATA_WIDTH/8)-1 : 0]
s00_axi_wstrb,
34 input wire s00_axi_wvalid,
35 output wire s00_axi_wready,
36 output wire [1 : 0] s00_axi_bresp,
37 output wire s00_axi_bvalid,
38 input wire s00_axi_bready,
39 input wire [C_S00_AXI_ADDR_WIDTH-1 : 0]
s00_axi_araddr,
40 input wire [2 : 0] s00_axi_arprot,
41 input wire s00_axi_arvalid,
42 output wire s00_axi_arready,

43 output wire [C_S00_AXI_DATA_WIDTH-1 : 0]

s00_axi_rdata,
44 output wire [1 : 0] s00_axi_rresp,
45 output wire s00_axi_rvalid,
46 input wire s00_axi_rready
47 );
48 // Instantiation of Axi Bus Interface S00_AXI
49 PWM_60hz_v1_0_S00_AXI # (
50 .C_S_AXI_DATA_WIDTH(C_S00_AXI_DATA_WIDTH),
51 .C_S_AXI_ADDR_WIDTH(C_S00_AXI_ADDR_WIDTH),
52 .PWM_COUNTER_MAX(PWM_COUNTER_MAX)
53 ) PWM_60hz_v1_0_S00_AXI_inst (
54 .PWM0(PWM0),
55 .PWM1(PWM1),
56 .PWM2(PWM2),
57 .PWM3(PWM3),
58 .S_AXI_ACLK(s00_axi_aclk),
59 .S_AXI_ARESETN(s00_axi_aresetn),
60 .S_AXI_AWADDR(s00_axi_awaddr),
61 .S_AXI_AWPROT(s00_axi_awprot),
62 .S_AXI_AWVALID(s00_axi_awvalid),
63 .S_AXI_AWREADY(s00_axi_awready),
64 .S_AXI_WDATA(s00_axi_wdata),
65 .S_AXI_WSTRB(s00_axi_wstrb),
66 .S_AXI_WVALID(s00_axi_wvalid),
67 .S_AXI_WREADY(s00_axi_wready),
68 .S_AXI_BRESP(s00_axi_bresp),
69 .S_AXI_BVALID(s00_axi_bvalid),
70 .S_AXI_BREADY(s00_axi_bready),
71 .S_AXI_ARADDR(s00_axi_araddr),
72 .S_AXI_ARPROT(s00_axi_arprot),
73 .S_AXI_ARVALID(s00_axi_arvalid),
74 .S_AXI_ARREADY(s00_axi_arready),
75 .S_AXI_RDATA(s00_axi_rdata),
76 .S_AXI_RRESP(s00_axi_rresp),
77 .S_AXI_RVALID(s00_axi_rvalid),
78 .S_AXI_RREADY(s00_axi_rready)
79 );
80
81 // Add user logic here

82
83 // User logic ends

84
85 endmodule

M. Código de ejemplo para PWM en Xilinx SDK

1 #include "xparameters.h"
2 #include "xil_io.h"
3 #include "xgpio.h"
4 #include "platform.h"
5 #include "sleep.h"
6 #include "stdio.h"
7
8 #define MY_PWM XPAR_PWM_60HZ_0_S00_AXI_BASEADDR

9 #define Full_Val 833334
10
11 int main(){
12 XGpio_Config *buttonConfig;
13 XGpio button;
14 int Status;
15 int i;
16 int val;
17 int max=12;
18 int min = 6;
19 int cont = 0;
20
21 init_platform();
22 buttonConfig = XGpio_LookupConfig(
XPAR_AXI_GPIO_0_DEVICE_ID);
23
24 Status = XGpio_CfgInitialize(&button, buttonConfig,

buttonConfig->BaseAddress);
25 XGpio_SetDataDirection(&button, 1, 0b1);
26
27
28 while(1){
29 val = XGpio_DiscreteRead(&button, 1);
30 printf(" %d \n\r",val);
31 if(val == 1){
32 if(cont = 0){
33 cont = 1;
34 }
35 else{
36 cont = 0;
37 }
38 }
39 printf(" %d \n \r",cont);
40 switch(cont){
41 case 0:
42 i = min;

43 break;
44 case 1:
45 i = max;
46 break;
47 }
48 Xil_Out32(MY_PWM, Full_Val*(i/100.0));
52 usleep(200000);
53 }
54 }

N. Diagrama de relaciones ROS del proyecto
Fig. N.1: Diagrama de relaciones completo en comunicación ROS para el presente proyecto

Ñ. Código con monitoreo por puerto serial para con-

trolador de vuelo
1 #include <stdio.h> //STANDARD IO
2 #include "xscutimer.h" //Timer lib.
3 #include "sleep.h" //To implement delay
4 #include "xil_printf.h" //To print in console
5 #include "xiic.h" //Iic functions
6 #include <math.h> //Math operations
7 #include "BME280_IIC.h" //Barometer lib
8 #include "BNO080_IIC.h"
10 #include "xspi.h"
11 #include "xil_exception.h"
12 #include "xscugic.h"
15 #include "PID.h"
16
17 //Se definen parámetros de pid

18 #define PID_KP 1000.0f
19 #define PID_KI 200.0f
20 #define PID_KD 126.0f
21
22 #define PID_TAU 0.2f

23
24 #define PID_LIM_MIN 0.0f

25 #define PID_LIM_MAX 50000.0f
26
27 #define PID_LIM_MIN_INT 12750.0f

28 #define PID_LIM_MAX_INT 37750.0f
29
30 #define SAMPLE_TIME_S 0.1f

31
32 //PWM Setup
35
36
37 BME280 Barometer;
38 BNO080 IMU;
39
40 #define SPI_DEVICE_ID XPAR_SPI_0_DEVICE_ID

41 #define INTC_DEVICE_ID XPAR_PS7_SCUGIC_0_DEVICE_ID
42 #define SPI_IRPT_INTR XPAR_FABRIC_SPI_0_VEC_ID

43 #define BUFFER_SIZE 6
44
45 /************************** Function Prototypes

******************************/
46
47 static int SetupInterruptSystem(XSpi *SpiInstance);

48
49 static void SpiHandler(void *CallBackRef, u32 StatusEvent,

50 unsigned int ByteCount);
51
52 void PIDforH();
53 /************************** Variable Definitions
*****************************/
54
55 static XSpi SpiInstance; /* SPI */

56 static XScuGic IntcInstance;
57
58 u8 ReadBuffer[BUFFER_SIZE];
59 u8 WriteBuffer[BUFFER_SIZE];
60
61 static volatile int TransferInProgress;

62 u8 ControlSignal;
63
64 /***********************Variables de almacenamiento temporal

***************/
65 float RetAlt = 0;
66 float RetQuatX = 0;
67 float RetQuatY = 0;
68 float RetQuatZ = 0;
69 float RetQuatR = 0;
70 float RetVelX = 0;
71 float RetVelY = 0;
72 float RetVelZ = 0;
73 float RetAccX = 0;
74 float RetAccY = 0;
75 float RetAccZ = 0;
76
77 float TrueZ = 0;
78 float baseZ = 0;
79 float acumZ = 0;
80 float reqZ = 1.5;
81 float T = 0;
82 float P = 0;
83 float R = 0;
84 float Y = 0;
85

86 //PWM limits
87 float maxThrottle = 12*(Full_Val/100.0);
88 float minThrottle = 6*(Full_Val/100.0);
89
90 //Inicializacion pid
91 PIDController pid = { PID_KP, PID_KI, PID_KD,
92 PID_TAU,
93 PID_LIM_MIN,
PID_LIM_MAX,
94 PID_LIM_MIN_INT, PID_LIM_MAX_INT,
95 SAMPLE_TIME_S };
96
97
98
99
100 int main()

101 {
102
103 //Gpio Setup

104 XGpio_Config *ioConfig;
105 XGpio io;
106 //SPI Setup
107 XSpi_Config *ConfigPtr;
108 int count = 0;
109 int Status;
110 u8 EN = 0;
111
112 u8 stat;
113 u8 AuxBuff[sizeof(float)];
114 u8 ReadAuxB[sizeof(float)];
115
116 float Alt;

117 float QuatX;
118 float QuatY;
119 float QuatZ;
120 float QuatR;
121 float VelX;
122 float VelY;
123 float VelZ;
124 float AccX;
125 float AccY;
126 float AccZ;
127 bool setpnt = false;
128 bool giroinit = false;
129 float TH1,TH2,TH3,TH4;
130 int initcount = 0;

131
132
133 PIDController_Init(&pid);
134
135 //Inicio SPI comm

136 xil_printf("\r\nEntering the Spi Slave intr Example.\
r\n");
137 xil_printf("Waiting for data from SPI master\r\n");
138 ConfigPtr = XSpi_LookupConfig(SPI_DEVICE_ID);
139 if (ConfigPtr == NULL) {
140 return XST_FAILURE;
141 }
142 Status = XSpi_CfgInitialize(&SpiInstance, ConfigPtr,
143 ConfigPtr->BaseAddress);
144 if (Status != XST_SUCCESS) {
146 }
147 Status = SetupInterruptSystem(&SpiInstance);
150 }
151 XSpi_SetStatusHandler(&SpiInstance,&SpiInstance,(
XSpi_StatusHandler)
152 SpiHandler);
153 Status = XSpi_SetOptions(&SpiInstance, 0x00);
156 }
157 XSpi_Start(&SpiInstance);
158 XSpi_IntrEnable(&SpiInstance,
XSP_INTR_TX_HALF_EMPTY_MASK);
159
160 //Inicio barómetro

161 print("Iniciando sensor de altitud \n\r");
162 stat = Barometer.inicio();
163 if(stat != TRUE)
164 {
165 print("Error con el sensor\n\r");
166 printf(" %d \n\r",stat);
167 return 1;
168 }
169 //Inicio IMU
170 print("Iniciando IMU \n\r");
171 if(IMU.begin(BNO_ADDRESS) == false){
172 printf("error check wiring");
173 return 3;

174 }
175 printf("begin done\r\n");
176 //Activación lecturas IMU
177 IMU.enableRotationVector(50);
178 printf("Rot. vector enabled\r\n");
179 IMU.enableLinearAccelerometer(50);
180 printf("Lin. Accel enabled\r\n");
181 IMU.enableGyro(50);
182 printf("Ang. Vel. enabled\r\n");
183
184 //GPIO config

185 ioConfig = XGpio_LookupConfig(
186 Status = XGpio_CfgInitialize(&io, ioConfig, ioConfig
->BaseAddress);
187 XGpio_SetDataDirection(&io, 1, 0b11);
189 XGpio_DiscreteWrite(&io, 2, 0b0);
190
191 //Motors INIT

192 Xil_Out32(MY_PWM, minThrottle);
193 Xil_Out32((MY_PWM+4), minThrottle);
196 TH1 = minThrottle+2500;
200 //sleep(4);
201 while(1){
202 if(EN == 0){
203 EN = XGpio_DiscreteRead(&io, 1);
204 //printf("a %x \n\r",EN);
205 EN &= 0b01;
206 }
207 if(EN == 1)
208 {
210 ControlSignal = ReadBuffer[0];
211 WriteBuffer[0] = ControlSignal;
212 //printf("Cont. signal: %x\r\n",
ControlSignal);
213 //xil_printf("ready\n\r");
214 /* WriteBuffer[1] = 0x79;
215 WriteBuffer[2] = 0x54;

217 WriteBuffer[4] = 0x26;*/

219 Alt = Barometer.readFloatAltitude();
220 //printf("Altitude: ");
221 //printf(" %3.3f mt\r\n",Alt);
222 if(IMU.dataAvailable()==true){
223 QuatX = IMU.getQuatI();
224 QuatY = IMU.getQuatJ();
225 QuatZ = IMU.getQuatK();
226 QuatR = IMU.getQuatReal();
227 VelX = IMU.getGyroX();
228 VelY = IMU.getGyroY();
229 VelZ = IMU.getGyroZ();
230 AccX = IMU.getLinAccelX();
231 AccY = IMU.getLinAccelY();
232 AccZ = IMU.getLinAccelZ();
233 }
234 /*printf("Rotation vector components:
");
235 printf(" %4.4f, %4.4f, %4.4f, %4.4f\t
\r\n", QuatX, QuatY, QuatZ, QuatR);
236 printf("Rotation vector components:
");
237 printf(" %4.4f, %4.4f, %4.4f\t \r\n",
VelX, VelY, VelZ);
238 printf("Rotation vector components:
");
239 printf(" %4.4f, %4.4f, %4.4f\t \r\n",
AccX, AccY, AccZ);*/
240 switch(ControlSignal){
241 case 0x01:
242 *(float *)(AuxBuff) = RetAlt;
243 //printf("enviando Alt: %lf\r
\n", RetAlt);
244 break;
245 case 0x02:
246 *(float *)(AuxBuff) =
RetQuatX;
247 //printf("enviando QuatX: %lf
\r\n", RetQuatX);
248 break;
249 case 0x03:
RetQuatY;
251 //printf("enviando QuatY: %lf
\r\n", RetQuatY);

252 break;
253 case 0x04:
RetQuatZ;
255 //printf("enviando QuatZ: %lf
\r\n", RetQuatZ);
256 break;
257 case 0x05:
RetQuatR;
259 //printf("enviando QuatR: %lf
\r\n", RetQuatR);
260 break;
261 case 0x06:
262 *(float *)(AuxBuff) = RetVelX
;
263 //printf("enviando VelX: %lf\
r\n", RetVelX);
264 break;
265 case 0x07:
266 *(float *)(AuxBuff) = RetVelY
;
267 //printf("enviando VelY: %lf\
r\n", RetVelY);
268 break;
269 case 0x08:
270 *(float *)(AuxBuff) = RetVelZ
;
271 //printf("enviando VelZ: %lf\
r\n", RetVelZ);
272 break;
273 case 0x09:
274 *(float *)(AuxBuff) = RetAccX
;
275 //printf("enviando AccX: %lf\
r\n", RetAccX);
276 break;
277 case 0x0A:
278 *(float *)(AuxBuff) = RetAccY
;
279 //printf("enviando AccY: %lf\
r\n", RetAccY);
280 break;
281 case 0x0B:
282 *(float *)(AuxBuff) = RetAccZ
;

283 //printf("enviando AccZ: %lf\

r\n", RetAccZ);
284 break;
285 case 0x0C:
286 /*printf("Resumen:\r\n");
287 printf("- %4.4f\r\n", RetAlt);
288 printf("- %lf\r\n", RetQuatX);
289 printf("- %lf\r\n", RetQuatY);
290 printf("- %lf\r\n", RetQuatZ);
291 printf("- %lf\r\n", RetQuatR);
292 printf("- %lf\r\n", RetVelX);
293 printf("- %lf\r\n", RetVelY);
294 printf("- %lf\r\n", RetVelZ);
295 printf("- %lf\r\n", RetAccX);
296 printf("- %lf\r\n", RetAccY);
297 printf("- %lf\r\n", RetAccZ);
*/
298 break;
299
300 case 0x0D:

301 for (int m=0; m<sizeof(float)
;m++){
302 ReadAuxB[m] =
ReadBuffer[1+m];
303 }
304 TrueZ = *(float *)(ReadAuxB);
305 printf("Real = %lf\n\r",TrueZ
);
306 if(setpnt == false){
307 if(TrueZ == 0){
308 initcount =
initcount;
309 acumZ = 0;
310 }
311 else{
312 acumZ = acumZ
+ TrueZ;
313 initcount =
initcount +
1;
314 }
315 //printf("acum = %lf\
n\r",acumZ);
316 if(initcount >= 10){
317 //Se calcula
el promedio

de las
alturas
obtenidas
318 baseZ = acumZ
/((
initcount -
1) * 1.0);
319 printf("base
= %lf\n\r",
baseZ);
320 giroinit =
true;
321 setpnt = true
;
322 }
323
324 }
325 //printf("Altura real es: %lf
", TrueZ);
326 //return 0;
327 break;
328
329 default:
330 AuxBuff[0] = 0x00;
331 AuxBuff[1] = 0x00;
332 AuxBuff[2] = 0x00;
333 AuxBuff[3] = 0x00;
334 RetAlt = Alt;
335 RetQuatX = QuatX;
336 RetQuatY = QuatY;
337 RetQuatZ = QuatZ;
338 RetQuatR = QuatR;
339 RetVelX = VelX;
340 RetVelY = VelY;
341 RetVelZ = VelZ;
342 RetAccX = AccX;
343 RetAccY = AccY;
344 RetAccZ = AccZ;
345 //xil_printf("esperando....\
r\n");
346 break;
347 }
348 for(u8 k = 0; k<sizeof(float); k++){
349 WriteBuffer[k+1] =
AuxBuff[k];
350 }

351
352 //PID controller

353 PIDforH();
354
355 //MMA
356 TH1 = (T + P + R + Y);
357 if (TH1 < minThrottle+2500){
359 }
360 if (TH1 > maxThrottle){
361 TH1 = maxThrottle;
362 }
363 TH2 = (T + P - R - Y);
366 }
369 }
370 TH3 = (T - P - R + Y);
373 }
376 }
377 TH4 = (T - P + R - Y);
380 }
383 }
384 //MOTOR OUTPUTS
385 if (giroinit == false){
386 Xil_Out32(MY_PWM, minThrottle
+2500);
387 Xil_Out32((MY_PWM+4),
minThrottle+2500);
minThrottle+2500);
minThrottle+2500);
390 }
391 else{
392 Xil_Out32(MY_PWM, TH1);

393 Xil_Out32((MY_PWM+4), TH2);

396 }
397
398
399 //usleep(100);
400
401 /*if(count >= 100){

402 printf("Done \r\n");
403 return 0;
404 }
405 count++;*/
406 }
407 TransferInProgress = TRUE;
408 Status = XSpi_Transfer(&SpiInstance,
409 WriteBuffer, ReadBuffer,
BUFFER_SIZE);
410 while(TransferInProgress);
411 }
412 }
413
414 static int SetupInterruptSystem(XSpi *SpiInstance)

415 {
416 int Status;
417 XScuGic_Config *Gic_Config;
418
419 Gic_Config = XScuGic_LookupConfig(INTC_DEVICE_ID);

420
421
422 /*
423 * Initialize the interrupt controller driver so that
it’s ready to use,
424 * specify the device ID that is generated in "
xparameters.h".
425 */
426 Status = XScuGic_CfgInitialize(&IntcInstance,
Gic_Config, Gic_Config->CpuBaseAddress);
429 }
430
431 /*
432 * Initialize the exception table.
433 */
434 Xil_ExceptionInit();

435
436 /*
437 * Register the interrupt controller handler with the
exception table.
438 */
439 Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_IRQ_INT
,
440 (Xil_ExceptionHandler)
XScuGic_InterruptHandler,
441 &IntcInstance);
442
443 /*
444 * Connect a device driver handler that will be
called when an interrupt
445 * for the device occurs, the device driver handler
performs the
446 * specific interrupt processing for the device.
447 */
448 Status = XScuGic_Connect(&IntcInstance,
449 SPI_IRPT_INTR,
450 (XInterruptHandler)
XSpi_InterruptHandler,
451 (void *)SpiInstance);
454 }
455
456 XScuGic_Enable(&IntcInstance, SPI_IRPT_INTR);

457 //XScuTimer_EnableInterrupt(&my_Timer);
458
459 /*
460 * Enable non-critical exceptions.
461 */
462 Xil_ExceptionEnable();
463
464 return XST_SUCCESS;

465 }
466
467 static void SpiHandler(void *CallBackRef, u32 StatusEvent,

468 unsigned int ByteCount)
469 {
470 //xil_printf("...");
471 if (StatusEvent == XST_SPI_TRANSFER_DONE) {
472 /*
473 * Indicate the transfer on the SPI bus is no
longer in

474 * progress.
475 */
476
477 TransferInProgress = FALSE;

478 //printf("imprimiendo recibido\r\n");
479 /*for(int k=0;k<BUFFER_SIZE;k++){
480 printf(" %x\r\n",ReadBuffer[k]);
481 }*/
482 //ControlSignal = ReadBuffer[0];
483 //xil_printf("Cont. signal: %d\r\n",
ControlSignal);
484 }
485 }
486
487 void PIDforH(){

488 float setpoint = 3.0f;
489 float Hforpid = 0;
490 Hforpid = TrueZ - baseZ;
491 T = PIDController_Update(&pid, setpoint, Hforpid);
492 T = T + minThrottle;
493 printf("T = %lf \r\n", T);
494 }

O. Código en lazo abierto para cálculo de compensa-

ciones por motor.
1 /*
2 * Empty C++ Application
3 */
4 #include <stdio.h> //STANDARD IO
5 #include "xscutimer.h" //Timer lib.
6 #include "sleep.h" //To implement delay
7 #include "xil_printf.h" //To print in console
8 #include <math.h> //Math operations
10 #include "xil_exception.h"
11 #include "xscugic.h"
14

17
18 #define comp 4650

19 #define comp2 1300
20 #define INTC_DEVICE_ID XPAR_PS7_SCUGIC_0_DEVICE_ID
21
22
23 /************************** Function Prototypes

******************************/
24 static void my_timer_interrupt_handler(void *CallBackRef);
25
26
27 // Global variables
28 int InterruptCounter = 0;
29
30 /************************** Variable Definitions

*****************************/
31
32
33 float maxThrottle = 12*(Full_Val/100.0);

34 float minThrottle = 6*(Full_Val/100.0);
35
36 int main()
37 {
38 int T;
39 int Status;
40 u8 EN = 0;

41 //Timer init
42 XScuTimer timerDeControl;
43 XScuTimer_Config *Tim_Config;
44 //Timeout interr controller
45 XScuGic timeout_gic;
46 XScuGic_Config *togic_Config;
47
48 togic_Config = XScuGic_LookupConfig(INTC_DEVICE_ID);
49 Status = XScuGic_CfgInitialize(&timeout_gic,
togic_Config, togic_Config->CpuBaseAddress);
50 Tim_Config = XScuTimer_LookupConfig(
XPAR_PS7_SCUTIMER_0_DEVICE_ID);
51 Status = XScuTimer_CfgInitialize(&timerDeControl,
Tim_Config, Tim_Config->BaseAddr);
52 Xil_ExceptionInit();
53 Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_IRQ_INT
, (Xil_ExceptionHandler)XScuGic_InterruptHandler, &
timeout_gic);
54 Status = XScuGic_Connect(&timeout_gic,
XPAR_SCUTIMER_INTR, (Xil_ExceptionHandler)
my_timer_interrupt_handler, (void *)&timerDeControl
);
55 XScuGic_Enable(&timeout_gic, XPAR_SCUTIMER_INTR);
56 XScuTimer_EnableInterrupt(&timerDeControl);
57 Xil_ExceptionEnable();
58 XScuTimer_LoadTimer(&timerDeControl,
XPAR_PS7_CORTEXA9_0_CPU_CLK_FREQ_HZ / 2);
59 XScuTimer_EnableAutoReload(&timerDeControl);
60
61
62 //Gpio Setup
63 XGpio_Config *ioConfig;
64 XGpio io;
65 float TH1,TH2,TH3,TH4;
66
67 //GPIO config
68 ioConfig = XGpio_LookupConfig(
69 Status = XGpio_CfgInitialize(&io, ioConfig, ioConfig
->BaseAddress);
73
74 //Motors INIT
75 Xil_Out32(MY_PWM, minThrottle);

83
84 while(1){
85 if(EN == 0){
86 EN = XGpio_DiscreteRead(&io, 1);
87 EN &= 0b01;
88 }
89 if(EN == 1)
90 {
91 XScuTimer_Start(&timerDeControl);
92 if (InterruptCounter <= 3){
93 T = 53000;
94 }
95 else if (InterruptCounter <= 6){
96 T = 80000;
97 }
98 else if(InterruptCounter <= 9){
99 T = 65000;
100 }
101 else if(InterruptCounter > 9){
102 T = 0;
103 }
104 if (InterruptCounter > 3 &&
InterruptCounter <= 6){
105 TH1 = (T + comp -comp2);
108 }
110 TH1 = maxThrottle
-28000;
111 }
112 TH2 = T - comp - comp2;
115 }
-28000;
118 }

119 TH3 = T - comp + comp2;

122 }
-28000;
125 }
126 TH4 = (T + comp + comp2);
129 }
-28000;//
132 }
133 }
134 else{
135 TH1 = (T );
138 }
-28000;
141 }
142 TH2 = T;
145 }
-28000;
148 }
149 TH3 = T;
152 }
-28000;
155 }
156 TH4 = (T);
159 }


-28000;
162 }
163 }
164 Xil_Out32(MY_PWM, TH1);
168
169
170 }
171 }
172 }
173
174 static void my_timer_interrupt_handler(void *CallBackRef){

175
176 XScuTimer *my_Timer_LOCAL = (XScuTimer *) CallBackRef

;
177
178 if (XScuTimer_IsExpired(my_Timer_LOCAL))
179 {
180 XScuTimer_ClearInterruptStatus(my_Timer_LOCAL
);
181
182 InterruptCounter++;
183 }
184 }

Dise No y Construcci On de Drone para Tareas de Inspecci On en Mantenimiento

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Dise No y Construcci On de Drone para Tareas de Inspecci On en Mantenimiento

Cargado por

Copyright:

Formatos disponibles

Facultad de Ingenierı́a

Escuela de Ingenierı́a Civil Mecatrónica

Diseño y construcción de drone para tareas

Gonzalo Rojas Garcı́a

forma total o parcial e ilimitada del mismo.

Vicerrectoría Académica | Dirección de Bibliotecas

Gonzalo Rojas Garcı́a

Gonzalo Rojas Garcı́a i

Gonzalo Rojas Garcı́a ii

Gonzalo Rojas Garcı́a iii

3.1.11 Sensor ultrasónico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

6 Conclusiones y trabajo futuro 90

A Control sobre Roll, Pitch, Yaw y Thrust 96

B Control sobre posición en ejes X, Y y Z 97

C Plano de placa de montaje para Cora Z7 98

D Código ejemplo para uso de barómetro 99

E Código ejemplo para uso de la IMU 100

F Código ejemplo para uso de sensor GPS 103

G Biblioteca .h para barómetro 105

H Código .cc para barómetro 109

I Biblioteca .h para IMU 120

J Código .cc para IMU 128

K Código AXI-PWM.v 167

Gonzalo Rojas Garcı́a v

L Código PWM.v 178

M Código de ejemplo para PWM en Xilinx SDK 180

N Diagrama de relaciones ROS del proyecto 182

O Código en lazo abierto para cálculo de compensaciones por motor. 196

Gonzalo Rojas Garcı́a vi

4.4 Análisis de deflexión sobre la cubierta del UAV con 6 soportes. . . . . . . 45

Gonzalo Rojas Garcı́a viii

ESC Electronic Speed Controller

FET Field Effect Transistor

GPS Global Positioning System

I2C Inter Integrated Circuits

LiPo Lithium Polymer

MMA Motor Mixing Algorithm

NMEA National Marine Electronics Association

RAM Random Access Memory

SHTP Sensor Hub Transfer Protocol

UART Universal Asynchronous Receiver-Transmitter

Gonzalo Rojas Garcı́a x

VHDL Very High Speed Integrated Circuit (VHSIC)

Gonzalo Rojas Garcı́a xi

Gonzalo Rojas Garcı́a 1

desarrollo, enfocándose en la parte inicial correspondiente a construcción, programación y

1.2. Estado del arte

Gonzalo Rojas Garcı́a 2

1.2.2. Vehı́culos aéreos no tripulados (UAV)

Gonzalo Rojas Garcı́a 3

Fig. 1.1: Diagrama de principales tipos de UAV’s

Gonzalo Rojas Garcı́a 4

Fig. 1.2: Diagrama de principales tipos de UAV’s de hélice rotatoria

Cabe destacar que, si bien, en la imagen no se presenta el UAV trihélice, su disposición

Gonzalo Rojas Garcı́a 5

la calidad del aire, exploración en minerı́a, seguridad y control de fronteras, mantenimiento

1.2.3. Estrategias de control

Gonzalo Rojas Garcı́a 6

Fig. 1.3: Tipos de control más usados en UAV’s

Gonzalo Rojas Garcı́a 7

Gonzalo Rojas Garcı́a 8

su ventaja radica en mantener la salida en las condiciones operacionales. No obstante, se

Fig. 1.4: Diagrama de bloques para controlador fuzzy