Interfície LabView per programar el sistema de control de quadrotors

TRABAJO FINAL DE CARRERA

TITULO DEL TFC : Interfaz LabView para programar el sistema de control dequadrotores

TITULACION: Doble titulacion : Ingenierıa Tecnica de Telecomunicaciones, es-pecialidad en Sistemas de Telecomunicacion e Ingenierıa Tecnica Aeronautica,especialidad en Aeronavegacion

AUTOR: Jordi Bracke Manzanares

DIRECTOR: Oscar Casas Piedrafita

FECHA: 22 de julio de 2010

Tıtulo : Interfaz LabView para programar el sistema de control de quadrotores

Autor: Jordi Bracke Manzanares

Director: Oscar Casas Piedrafita

Fecha: 22 de julio de 2010

Resumen

En este trabajo se desarrolla el diseno y construccion del software de abordo de unquadrotor. Se explican los pasos necesarios para estabilizarlo y controlarlo ası como tam-bien la creacion de una interfaz de prototipado para probar nuevas tecnicas de control yguiado. Dicha interfaz se basa en un quadrotor comercial con las mınimas modificacionesposibles conectado a un ordenador personal.

La implementacion se ha realizado sobre el sistema que comercializa Mikrokopter. Seha eliminado el firmware de la unidad de vuelo y se ha disenado uno que a traves delpuerto serie envıe los datos de los sensores de abordo y reciba unos valores de potenciaa entregar a los motores. Mediante un conversor serie-usb se ha conectado la unidad devuelo a un ordenador portatil. El software de vuelo de ejecuta en un PC y se ha escrito enLabview dado que es muy indicado para control y visualizacion de datos.

Este sistema permite acelerar enormemente el desarrollo de software de control dequadrotores. La capacidad de visualizar de forma grafica los datos de los sensores de launidad de vuelo simplifica la deteccion de errores de diseno y refuerza la realimentacionque experimenta el usuario con su diseno. La capa de abstraccion que ofrece el sistemaevita al disenador tener que tratar directamente con el hardware de la unidad de vuelo yla potencia de calculo del PC permite al disenador el uso de coma flotante casi sin restric-ciones. Por otra parte el uso de Labview evita al programador tener que tratar con uno delos errores mas comunes de C: el mal uso de los punteros.

Title : Labview interface to program a quadrotor’s control system

Author: Jordi Bracke Manzanares

Director: Oscar Casas Piedrafita

Date: July 22, 2010

Overview

This project is about the design of quadrotor’s fligth software. All necessary steps in orderto fly and control it are explained as well as the creation of an interface to prototype controland guidance techniques. The interface is based on a commercial quadrotor connected toa personal computer.

The implementation has been done on the Mikrokopter quadrotor. The firmware has beenremoved in order to install a new one with different capabilities. The new firmware iscapable of sending onboard sensor data and receiving motor power data from a personalcomputer. The communication has been done connecting a serial-usb converter fromthe quadrotor to a personal computer. The flight software is executed on the personalcomputer and it’s written in Labview as it’s very suitable for control and data visualization.

This system decreases the developing time of quadrotor’s flight software. The designercan visualize sensor data helping him to detect errors. The abstraction layer helps himnot to deal directly with hardware and the use of Labview prevents mistakes that are verycommon in C.

INDICE GENERAL

CAPITULO 1.Vehıculos aereos no tripulados . . . . . . . . . . . . . 11

1.1. UAVs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111.1.1. Definicion, historia y aplicaciones . . . . . . . . . . . . . . . . . . . 11

1.1.2. Quadrotor o quadricoptero . . . . . . . . . . . . . . . . . . . . . . 11

1.2. Quadrotores actuales . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

CAPITULO 2.Guıa para la estabilizacion de un quadrotor . . . . . . 13

2.1. Electronica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.2. Fusion de datos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.3. Tablas de mezclado (Mixer tables) . . . . . . . . . . . . . . . . . . . . . . 15

2.4. Controlador PID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

2.5. Piloto manual . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

CAPITULO 3. Implementacion del quadrotor . . . . . . . . . . . . . . . 21

3.1. Eleccion de la plataforma . . . . . . . . . . . . . . . . . . . . . . . . . . . 213.1.1. Quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

3.1.2. PC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.1.3. Lenguaje para la programacion del software de control . . . . . . . . 26

3.1.4. Enlace de comunicacion . . . . . . . . . . . . . . . . . . . . . . . . 27

3.1.5. Alimentacion del conjunto . . . . . . . . . . . . . . . . . . . . . . . 27

3.1.6. Esquema y fotografıa de la plataforma . . . . . . . . . . . . . . . . 29

3.2. Modificacion y programacion del firmware de la unidad de vuelo . . . . . 303.2.1. Descarga del codigo fuente del firmware de la Flight Control . . . . . 30

3.2.2. Instalacion del compilador y el gestor de Makefiles . . . . . . . . . . 31

3.2.3. Compilacion del codigo en Windows y programacion del microcon-trolador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

3.2.4. Comunicacion del quadrotor con el PC . . . . . . . . . . . . . . . . 33

3.3. Programacion del conjunto . . . . . . . . . . . . . . . . . . . . . . . . . . 373.3.1. Consideraciones al trabajar con varias plataformas . . . . . . . . . . 37

3.4. Detalles de implementacion . . . . . . . . . . . . . . . . . . . . . . . . . . 38

CAPITULO 4.Ampliacion de hardware . . . . . . . . . . . . . . . . . . . 41

4.1. Placa Navigation Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

4.2. GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 424.2.1. Fragmentacion para transportar los datos del GPS . . . . . . . . . . 42

4.3. Brujula . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 434.3.1. Protocolo de comunicacion . . . . . . . . . . . . . . . . . . . . . . 44

4.4. Sonar EZ0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 444.4.1. Modelo de eliminacion de falsas lecturas . . . . . . . . . . . . . . . 45

CAPITULO 5.Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . 47

INDICE DE FIGURAS

1.1 Estructura basica de un quadrotor . . . . . . . . . . . . . . . . . . . . . . . . 111.2 Draganflyer V5 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.1 Descomposicion de la aceleracion estatica y dinamica mediante filtrado . . . . 142.2 Descomposicion de la gravedad . . . . . . . . . . . . . . . . . . . . . . . . . 142.3 Esquema de las 4 maniobras que queremos controlar sobre un draganflyer . . 152.4 Mando Logitech utilizado para controlar los angulos . . . . . . . . . . . . . . . 19

3.1 Esquema de la plataforma: la placa del medio es un puente serie-usb . . . . . 213.2 Kit del Mikrokopter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.3 Placa Flight Control vista desde arriba y desde abajo . . . . . . . . . . . . . . 233.4 Brushless Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243.5 El conversor MKUSB . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243.6 Motor sin escobillas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243.7 MK3MAG brujula electronica . . . . . . . . . . . . . . . . . . . . . . . . . . . 253.8 MKGPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 253.9 Asus M51V . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.10Ejemplo de programacion grafica en Labview . . . . . . . . . . . . . . . . . . 263.11Fuente ATX . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 283.12Puente para encender la fuente ATX . . . . . . . . . . . . . . . . . . . . . . . 283.13Esquema de la interconexion de subsistemas y fotografıa de la plataforma . . . 293.14Tortoise SVN haciendo un checkout al repositorio de Mikrokopter . . . . . . . . 303.15Codigo de la Flight Control compilado con exito usando make 3.81 y WINAVR-

20100110 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 323.16Programando el microcontrolador usando el MK-Tool . . . . . . . . . . . . . . 323.17Reduccion de la latencia virtual que introduce el sistema operativo . . . . . . . 343.18Incremento de la latencia del sistema por medio de feedback nodes . . . . . . 353.19Adaptador Xbee USB . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353.20Cliente ping hecho en Labview para medir la latencia de los dos modulos . . . 363.21Latencia de la comunicacion entre dos modulos zigbee . . . . . . . . . . . . . 363.22Placa Nano ITX. Las dimensiones son perfectas para llevar en un quadrotor . . 363.23Big Endian vs Little Endian . . . . . . . . . . . . . . . . . . . . . . . . . . . . 373.24Intercambio de bytes en un array con ancho de palabra de 16 bits . . . . . . . 373.25Padding para que el tamano de la estructura sea multiplo de 32 bit (Linux) . . . 383.26Esqueleto en Labview para programar quadrotores . . . . . . . . . . . . . . . 393.27Apertura y configuracion del puerto serie en Labview . . . . . . . . . . . . . . 403.28Envıo del comando ’5’ al firmware . . . . . . . . . . . . . . . . . . . . . . . . 403.29Bucle principal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

4.1 NaviCtrl con MKGS y MK3MAG . . . . . . . . . . . . . . . . . . . . . . . . . 414.2 Esquema de la placa adaptadora . . . . . . . . . . . . . . . . . . . . . . . . . 414.3 Placa adaptadora montada . . . . . . . . . . . . . . . . . . . . . . . . . . . . 424.4 Al anadir informacion GPS la latencia pasa de ser casi constante a ser variable 424.5 Problema al anadir informacion GPS . . . . . . . . . . . . . . . . . . . . . . . 434.6 Eliminacion de falsas lecturas del sonar . . . . . . . . . . . . . . . . . . . . . 45

Vehıculos aereos no tripulados 11

CAPITULO 1. VEHICULOS AEREOS NOTRIPULADOS

1.1. UAVs

1.1.1. Definicion, historia y aplicaciones

Un UAV (del ingles Unmanned Aerial Vehicle) es un vehıculo aereo sin tripulacion humanaa bordo. Su potencial se empezo a desarrollar despues de la Primera Guerra Mundial.Fueron ampliamente utilizados en operaciones militares, tanto para entrenamiento comopara misiones. A pesar de tener su inicio en el ejercito tienen un gran potencial para apli-caciones de uso civil por lo que se han utilizado en campos como la vigilancia, telemetrıa,busqueda de civiles en catastrofes y fotografıa aerea entre otras, sin olvidar el aeromod-elismo.

1.1.2. Quadrotor o quadricoptero

Un quadrotor no es mas que una estructura simetrica que consiste en dos barras cruzadasen forma de + o x con motores en sus extremos. En lugar de usarse superficies de controlcomo en un avion, el control se efectua variando la potencia de los motores. Esto provocavariaciones en los momentos del quadrotor que dan lugar al movimiento que se deseaobtener. El sentido de giro de las helices en dos motores es horario mientras que en otrosdos es antihorario, de esta forma se conserva el par motor y evita que gire sobre si mismo.

Figura 1.1: Estructura basica de un quadrotor

Dentro de los UAVs los quadrotores estan cobrando cada vez mas importancia gracias asu gran maniobrabilidad y su capacidad VTOL (Vertical Take Off and Landing). Se puedenencontrar incluso en el ambito del consumidor a partir de 400 euros. Esto ha permiti-do que numerosas universidades lo utilicen como plataforma de investigacion tanto paraaplicaciones de control, robotica o incluso inteligencia artificial.

12 Interfaz LabView para programar el sistema de control de quadrotores

1.2. Quadrotores actuales

Hacer volar un quadrotor ha dejado de ser algo innovador para convertirse en un reto dadoque existen una gran cantidad de modelos. Dentro del ambito del consumidor algunoshan encontrado su vertiente comercial como Draganfly, otros a pesar de ser comercialesrequieren alrededor de 10 horas de montaje como es el caso de Mikrokopter y otros sinembargo se puede adquirir la lista de materiales y construirlo como el AeroQuad o elQuadcopter.

Figura 1.2: Draganflyer V5

Los objetivos de este proyecto son:

1. creacion de una interfaz de comunicacion entre un quadrotor comercial y un or-denador personal que permita la optimizacion a nivel de prototipo del software denavegacion y control.

2. creacion de un esqueleto en Labview que haga uso de dicha interfaz.

3. disenar el software de vuelo de un quadrotor sobre el esqueleto anterior. Este soft-ware tendra elementos programables que podran ajustarse/adaptarse a cada unode los prototipos de quadrotor que quiera utilizarse.

Guıa para la estabilizacion de un quadrotor 13

CAPITULO 2. GUIA PARA LA ESTABILIZACIONDE UN QUADROTOR

2.1. Electronica

La electronica permitira leer magnitudes fısicas y convertirlas a un valor digital ademas decontrolar los motores. Entre los sensores se pueden distinguir:

1. Giroscopios: Miden la velocidad angular.

2. Acelerometros: Miden la aceleracion. Con ellos se puede medir tambien la gravedad.

3. Brujula: Mide el campo magnetico, se utiliza para no perder la orientacion.

4. Sensor de presion: Mide la presion atmosferica. Permite conocer la altura aproxi-mada.

Pese a que los dos ultimos no son necesarios son recomendables.

Todos los sensores despues de acondicionarse se conectan al conversor A/D del micro-controlador. Este calculara una respuesta que se enviara a los motores. Es un sistemade lazo cerrado pues la realimentacion se produce a traves del entorno fısico por el quese mueve variando sus magnitudes. Por ejemplo la aplicacion de potencia a un motorgenerara un aumento/disminucion de la aceleracion y/o velocidad angular.

2.2. Fusion de datos

El objetivo principal para mantener el quadrotor estabilizado es mantener el angulo depitch y roll lo mas cercano posible a 0. Idealmente si las helices son perfectas, la estructurasimetrica y el angulo es 0 toda la componente que propulsion que proporciona el motorse utiliza para la ascension.

Por lo tanto si se desea conseguir el angulo cercano a 0 es importante que se conozcaeste con la mayor exactitud posible.

Para calcularlo se utilizaran giroscopios que proporcionan la velocidad angular. Integrandola velocidad angular se obtiene el angulo. Pero durante el proceso se integra tambien elruido que proviene tanto del conversor A/D como del propio giroscopio. Depende de lacalidad del giroscopio el error sera mayor o menor.

Al principio el error sera pequeno, pero es cuestion de segundos, minutos u horas (de-pende de la calidad del giroscopio y del conversor) que el error pase a ser inaceptable.Para evitar que este se propague se debe contrastar el angulo con otra fuente de infor-macion. Es aquı donde entran en juego los acelerometros que miden tanto aceleracion


estatica (gravedad) como la dinamica. El problema es que miden la suma de las dos porlo tanto se ha de recurrir a filtros paso alto para obtener la componente dinamica y filtrospaso bajo para obtener la componente de la gravedad como muestra la Figura 2.1

Figura 2.1: Descomposicion de la aceleracion estatica y dinamica mediante filtrado

Si se determina la componente gravitatoria que se tiene en cada eje se puede averiguar lainclinacion del cuerpo con una simple operacion trigonometrica. En la Figura 2.2 se puedeapreciar que a medida que aumenta la inclinacion el eje X experimenta una aceleracionestatica mayor.

Figura 2.2: Descomposicion de la gravedad

El problema que presenta este metodo es que es lento debido al retraso que introduceel filtro. Al ser un filtro paso bajo necesitara varias muestras para hacer un promediadocosa que provocara un retardo en el calculo de la componente gravitatoria y por tanto unretardo en el calculo del angulo. Una respuesta lenta puede provocar que no el sistemano pueda responder a tiempo para generar una correccion y que por tanto sea inestable.

En resumen: se tienen los giroscopios que permiten calcular un angulo lo suficientementerapido para poder estabilizar el sistema a costa de un error que crece en cada paso deintegracion y se tienen los acelerometros que permiten calcular un angulo con un errorque no va a crecer pero con un retraso considerable. La solucion consiste en fusionarambos datos. Se utilizara una matriz DCM (Direct Cosine Matrix) con fusion de datosde acelerometros a traves de un controlador PI [2]. El algoritmo se basa principalmenteen una matriz de rotacion, la cual se va actualizando multiplicando por una matriz deangulos pequenos.La realimentacion del angulo del acelerometro se hace a traves de uncontrolador PI. El detalle de funcionamiento se puede encontrar en diydrones.com*


*diydrones.com es una comunidad online amateur sobre UAVs. Se puede encontrar muchainformacion sobre navegacion,control,sensores... Un ejemplo lo podemos encontrar en elArduIMU Quadcopter de Jose Julio [1], el cual se uso como referencia para la imple-mentacion del algoritmo DCM

2.3. Tablas de mezclado (Mixer tables)

Una vez que se conocen los angulos del quadrotor con una cierta precision lo siguienteque hay que plantearse es de que manera se pueden variar. Controlar la potencia delos 4 motores puede resultar un tanto complicado por lo que debemos crear una capade abstraccion, en lugar de intentar controlar la potencia de cada motor se debe intentarcontrolar maniobras que se definen en la Figura 2.3:

1. Incrementar/Decrementar angulo de Pitch (rojo)

2. Incrementar/Decrementar angulo de Roll (verde pistacho)

3. Incrementar/Decrementar angulo de Yaw (naranja)

4. Incrementar altura (azul)

La idea es que se pueda actuar sobre una maniobra y la capa de abstraccion actue sobrelos motores.

Figura 2.3: Esquema de las 4 maniobras que queremos controlar sobre un draganflyer

Se van a desglosar que actuaciones requiere cada maniobra y se trataran como si el restode maniobras no existiera para luego aplicar superposicion.

1. Incrementar/Decrementar angulo de Pitch: Se coge como referencia incrementar irde 4 hacia 3 y decrementar ir de 3 a 4 (ver Figura 2.3). Para incrementar el angulose debe aumentar la potencia en el motor 4 a la vez que se reduce la potencia en


el motor 3. Al incrementar la potencia en el motor 4, la helice ganara velocidad yexperimentara un aumento en la sustentacion haciendo subir al brazo que sujeta elmotor. En el motor 3 sucedera lo contrario: la disminucion de potencia provocara unperdida de sustentacion y tendera a caer. El resultado que experimentara la estruc-tura es un incremento del angulo de Pitch. Para decrementar el angulo se hara lomismo al reves (incrementar potencia en 3 y decrementar en 4).

2. Incrementar/Decrementar angulo de Roll: Dada la simetrıa del quadrotor la manio-bra de Roll es igual a la de Pitch solo que cambiando los motores sobre los quese actuan. Se incrementa el angulo aumentando la potencia en 1 a la vez que sereduce en 2 y viceversa.

3. Incrementar/Decrementar angulo de Yaw: Para esta maniobra se debe conservar elbalance de Pitch y Roll. Es decir la potencia con la que se actue sobre el motor 4debera ser igual a la del motor 3 y la potencia con la que se actue sobre el motor 1debera ser igual a la del motor 2. Esto es ası porque no se desea que cada vez quese actue sobre la maniobra de Yaw se vean afectadas las de Pitch/Roll.

Por otra parte se debe ser consciente de que el balance de potencia neta sobre los4 motores debe ser 0 de otra forma la maniobra de ascension se vera afectada.

La realizacion de esta maniobra depende de como se tengan colocadas las helices.Dependiendo si las helices que estan en sentido horario estan en los motores 1 y2 o por el contrario estan en los motores 3 y 4. La idea general consiste en decre-mentar la potencia por igual en los motores que tengan las helices girando en unsentido a la vez que se incrementar la potencia por igual en los otros dos motoresy viceversa. De esta forma se descompensa el par de giro y provoca un incrementoen la velocidad angular. Los incrementos y decrementos que se hagan deben serde la misma magnitud para que se mantenga la potencia de ascension.

4. Incrementar altura: Esta maniobra es muy simple: consiste en aumentar la potenciade los 4 motores por igual.

La potencia obtenida para cada motor por cada una de las 4 maniobras se suma. Es decirla potencia obtenida para el motor 1 en la maniobra de Pitch se suma con la potenciaobtenida para el motor 1 en la maniobra de Roll y ası sucesivamente. La forma matematicade hacer esto corresponde a la multiplicacion de un vector por una matriz por esta razonse define un vector de maniobras, donde se establecera la potencia que hay que entregarpara hacer cada maniobra.

Las potencias negativas seran para decrementar el angulo y las potencias positivas seranpara incrementar el angulo.

Pm =

PΦ

PΘ

PΨ

Pt

(2.1)

Este vector de valores se multiplicara ahora por una tabla de mezclado.


Mixer =

0,75 0,75 0,75 0,750,25 0,25 −0,25 −0,250,25 0,25 0 0

0 0 0,25 0,25

(2.2)

Es importante tener en cuenta que los calculos se van a hacer en un PC y por tanto sepueden usar decimales, pero el uso de coma flotante en un microcontrolador penalizamucho en el rendimiento.

Para obtener la potencia total:

PT = Pm ·Mixer (2.3)

PT =

PΦ

PΘ

PΨ

Pt

·

0,75 0,75 0,75 0,750,25 0,25 −0,25 −0,250,25 0,25 0 0

0 0 0,25 0,25

=

PMotorFrontalPMotorTrasero

PMotorIzquierdoPMotorDerecho

(2.4)

Haciendo referencia a la Figura 2.3 el motor frontal es el 3, el trasero el 4, el izquierdo el1 y el derecho el 2.

2.4. Controlador PID

Ahora que ya se conoce el angulo y como actuar sobre los motores para obtener el angu-lo que se desea, se necesita un controlador que monitoree el angulo y actue sobre lasmaniobras definidas anteriormente para corregirlo. Existen una gran diversidad de contro-ladores. Los hay muy sofisticados que podran proporcionar respuestas mas suaves peropor su sencillez se va a utilizar en el controlador PID (Proporcional Integral Derivativo).

Un controlador PID basico dispone de una entrada y una salida

1. Error: es la diferencia entre la senal que se desea obtener y la que se tiene ac-tualmente. Por ejemplo cuando el quadrotor no este realizando maniobras se de-seara un angulo de Pitch y Roll de 0o. Si se produce una perturbacion en el eje dePitch este angulo sera mayor, por ejemplo 3o. El error que se tendrıa en este casoes de 3 grados.

2. Salida de actuacion: Es la salida sobre la que tiene que actuar para corregir el error.Por ejemplo los 3o anteriores en el eje de Pitch deberan ser corregidos con unamaniobra de Decrementar el angulo de Pitch.

Debe haber algun tipo de realimentacion entre la salida y la entrada. Generalmente seproduce a traves de un medio fısico. En el caso de quadrotor la realimentacion se produce


a traves del motor que provoca un cambio en las magnitudes fısicas que seran medidaspor el conjunto de sensores.

La salida del controlador PID se genera a partir de la suma de tres partes que se generana partir del error de entrada.

1. Proporcional: La parte proporcional sale de multiplicar el error por una constanteque llamaremos Kp. De esta manera sin error la salida serıa 0 y a medida que elerror crece se irıa aumentando el valor de salida. En el caso del ejemplo anterior, amedida que aumenta el angulo de Pitch la salida que controla la maniobra de Pitchaumenta su potencia para rectificar el angulo. Kp debe ser escogido de manera quese entregue potencia suficiente para corregir pero no demasiada para sobrepasarla correccion.

2. Integral: La parte integral sale de multiplicar la integral del error por una constanteque llamaremos Ki. La integral puede ser calculada sumando el error en cada tiem-po de muestreo. Esta parte es importante porque tiende a cancelar el error. Imag-inemos el caso de que tenemos un pequeno peso colgado de un eje del quadrotorcreando una descompensacion de peso. Probablemente la parte proporcional nosea capaz de corregir el angulo de error que esta produciendo ese peso descom-pensatorio. A medida que pasa el tiempo la integral del error ira creciendo y por tantola salida empezara a incrementar su valor. Aunque el error haya quedado comple-tamente corregido la salida conservara su valor porque la integral es ir sumando elerror y sumar 0 no altera el valor del resultado. Si la constante Ki es muy grandepuede provocar que un pequeno error haga crecer muy deprisa la salida de la parteintegral produciendo una sobreactuacion. Si por el contrario Ki es muy pequenopuede que no sea capaz de corregir a tiempo la descompesacion.

3. Derivativo: La parte derivativa sale de multiplicar la derivada del error por una con-stante que llamaremos Kd. La derivada puede ser calculada utilizando el valor delgiroscopio. La parte derivativa puede ayudar a corregir sobreactuaciones en el sis-tema ya que se anticipa a la salida para disminuir la actuacion a medida que el errorse anula. Si Kd es muy grande la correccion puede ser muy fuerte y puede impediral sistema alcanzar la correccion deseada pero si es muy pequena puede que nosuavice la salida lo suficiente y se produzcan oscilaciones.

Finalmente la salida total no es mas que el resultado de las tres partes de controlador.

e(t) = anguloDeseado−anguloActual(t) (2.5)

Salida = Kp · e(t)+Ki ·∫ t

0e(t)dt +Kd · e(t) (2.6)

Las constantes pueden ser calculadas a partir de algoritmos de aproximacion sucesivos.

Se usaran controladores PID independientes para cada maniobra. Su entrada error sera elangulo que deseamos obtener menos el angulo que proviene de la fusion de datos entrelos acelerometros y giroscopios. La salida del controlador ira al vector de potencias demaniobra (Formula 2.1).


Cuando se tenga el vector de potencias de maniobra completo, se multiplicara por la tablade mezclado (Formula 2.2) de manera que se obtendra la potencia que hemos de enviara cada motor.

2.5. Piloto manual

El ultimo paso consiste en dotar al sistema de algun mecanismo para poder guiarlo sobreun espacio 3D. Dicho mecanismo consiste en variar los angulo del quadrotor. Al inclinarlo,se pierde parte de la componente de ascension que pasa a ser de avance.

La manera de controlar los angulos es actuar directamente sobre la senal de error queentra al PID. Volviendo a la Formula 2.5,suponiendo que el sistema esta estabilizado (0o

Pitch y 0o Roll) , si se modifica la variable anguloDeseado el error dejara de ser 0 paraverse incrementado (o decrementado) . El PID intentara minimizar el error provocando queel quadrotor se incline sobre el eje en que se haya actuado. De esta manera el quadrotoralcanzara el angulo propuesto.

Figura 2.4: Mando Logitech utilizado para controlar los angulos

Se puede utilizar un mando como el de la Figura 2.4 y asignar a cada eje un angulo delquadrotor.


Implementacion del quadrotor 21

CAPITULO 3. IMPLEMENTACION DELQUADROTOR

Se pretende que la implementacion del quadrotor sea distinta de las planteadas tradi-cionalmente. Se cambiara el firmware de un quadrotor comercial para convertir la unidadde vuelo en un sistema que se dedique:

1. leer datos de los sensores y enviarlos a un PC

2. recibir la potencia calculada por el PC para cada motor y actuar sobre estos

Por tanto la electronica se comprara montada de manera que solo se desarrollara el soft-ware de vuelo y el firmware que comunicara la unidad de vuelo con el ordenador.

3.1. Eleccion de la plataforma

La plataforma consistira en:

1. Un quadrotor: Se entiende por quadrotor el chasis con los motores y la electronicade abordo necesaria para poder volar

2. Un ordenador donde se disenara y ejecutara el prototipo.

Figura 3.1: Esquema de la plataforma: la placa del medio es un puente serie-usb

3.1.1. Quadrotor

3.1.1.1. Eleccion

Para la eleccion del quadrotor sobre el que va a correr el sistema se han planteado unaserie de requisitos:


1. Facil de adquirir: ya que en caso de utilizarse para impartir clases se comprarıanvarios. No pueden haber problemas de disponibilidad. En caso de fallo de algu-na pieza de hardware tambien deben existir recambios y no deben ser difıciles deencontrar.

2. Facil de programar: a ser posible que la propia placa incorpore un bootloader. Estoevita a la universidad comprar equipos especıficos y permite cargar el nuevo soft-ware rapidamente.

3. Economico: para permitir la compra de mas equipos para impartir docencia con elmismo presupuesto. El precio no deberıa superar los 1000 por plataforma.

4. Facil de montar: que el montaje no sea complicado ni demasiado casero. A serposible que todo encaje perfectamente.

Tras barajar varias opciones finalmente se escogio el Mikrokopter porque:

1. El kit necesario para volar anadiendo la brujula y el GPS apenas supera los 1000euros.

2. Es muy popular por lo que encontrar repuestos es facil.

3. Se puede programar con el mismo adaptador USB que viene con el kit. Este mismoadaptador sirve para establecer una interfaz con el PC.

4. El codigo fuente se puede descargar en el repositorio del la pagina web de Mikrokopter.

5. La calidad de los componentes del hardware es mayor que en otros productos delmismo precio.

6. La potencia de los motores es muy buena lo que permite levantar hasta mediokilogramo de peso.

7. Es facil de montar. Las placas encajan perfectamente en el chasis que te viene enel kit. Las placas de expansion tambien encajan perfectamente en el chasis.

3.1.1.2. Descripcion

En este apartado se describen los modulos de los cuales consta el quadrotor Mikrokopter.

El kit se compone de:

1. Flight Control: La unidad principal de vuelo. Contiene 3 giroscopios ADXRS610 y unacelero metro de 3 ejes LIS344ALH. Opcionalmente se le puede instalar un sensorde presion. Su procesador es un ATMEGA644P con una frecuencia de reloj de 20Mhz. Incorpora un bootloader que permite actualizar el firmware a traves del puertoUSB.

Como interfaces accesibles sin necesidad de soldadura hay dos UARTs, un puertoSPI y un puerto I2C. Tambien dispone de dos pines uno de los cuales es utilizadocomo SLAVE SELECT para el puerto SPI y otro puede ser utilizado para PWM.


Figura 3.2: Kit del Mikrokopter

Figura 3.3: Placa Flight Control vista desde arriba y desde abajo


2. Brushless Controller: Las controladoras de motores sin escobillas. Funcionan atraves de I2C o PPM. Pueden soportar una potencia de 110 W en contınua y un220 W de pico trabajando a 11.1 V. Disponen de medidor de corriente incorporado.

Figura 3.4: Brushless Controller

3. MKUSB: Es un simple conversor serie-USB basado en el famoso FT232RL. Propor-ciona un conector que se puede conectar directamente a la Flight Control.

Figura 3.5: El conversor MKUSB

4. Motores sin escobillas,chasis,cables... y todo lo necesario para montar el quadrotor.

Figura 3.6: Motor sin escobillas


5. Componentes opcionales:

(a) MK3MAG: Brujula electronica. Lleva 3 sensores de campo magnetico para de-terminar el norte y proporcionar informacion sobre el heading. Recomendablepara corregir la deriva en el angulo de Yaw debido al error de integracion. Esel unico componente que no se puede anclar bien a la estructura.

Figura 3.7: MK3MAG brujula electronica

(b) MKGPS: Lleva un GPS de la companıa u-blox capaz de enviar codigo de in-formacion NMEA 0183 ası como codigos propietarios que estan descritos enel datasheet. Ancla perfectamente a la estructura.

Figura 3.8: MKGPS


3.1.2. PC

El PC es el terminal que soportara toda la carga de procesado del sistema. Se ha utilizadoun portatil de gama media-baja ASUS M51V. Su precio en 2009 fue de 600 euros. Cuentacon un procesador de doble nucleo P8400 y 4 GB de RAM. El sistema operativo instaladoes Windows 7. Es capaz de procesar los datos de los sensores en tiempo real.

Figura 3.9: Asus M51V

3.1.3. Lenguaje para la programacion del software de control

El software de control se ejecutara en un PC que se comunicara con el quadrotor recibi-endo datos en tiempo real de los sensores y enviando datos de potencia a los motores. Elfirmware del microcontrolador del quadrotor se modificara para que pueda cumplir dichafuncion.

Se escogio Labview para programar el software de control ya que facilita mucho la vi-sualizacion de los datos . Con los modulos adicionales como el kit de Vision, Control ode Robotica se pueden anadir mas funcionalidades acelerando el diseno de manera muysignificativa.

Figura 3.10: Ejemplo de programacion grafica en Labview

No obstante el C es el lenguaje por excelencia para aplicaciones embebidas de bajocoste por lo que unicamente se pretende que sea una plataforma para la programacion


de prototipos.

Se ha desarrollado un esqueleto en C para facilitar la portabilidad del diseno de Labviewa C (una vez finalizado el diseno) que provee de las siguientes funcionalidades:

1. Comunicacion con el Mikrokopter: funciones de apertura de puerto serie y envıo dedatos. Incluye rutinas de codificacion.

2. Red: para enviar y recibir datos por UDP a Labview

3. Joystick: para leer la entrada de Joystick a traves de DirectX

De esta manera el planteamiento sugerido para la programacion del software de un quadro-tor serıa el siguiente:

1. Disenar el prototipo en Labview (PC)

2. Verificar el correcto funcionamiento del sistema

3. Portar el diseno a C en un PC

4. Verificar la correcta implementacion en C del prototipo

5. Hacer los cambios necesarios para compilar en codigo en la plataforma objetivo(compilacion cruzada)

3.1.4. Enlace de comunicacion

La comunicacion se hara por puerto serie debido a que es mas simple que la utilizacion deotros puertos como puede ser el USB. No obstante se utilizara un conversor serie-USB yaque la mayorıa de portatiles no incorporan ya puerto serie. Dicho conversor viene incluidoen el Kit de Mikrokopter y esta basado en el chip FT232RL que proporciona a traves desus drivers un puerto serie de manera transparente.

3.1.5. Alimentacion del conjunto

La forma directa de alimentar el conjunto es mediante la baterıa que se proporciona enel kit. El problema es que con los motores encendidos el consumo es muy elevado y labaterıa rara vez alcanzara los 20 minutos de duracion. El tiempo mınimo de carga de labaterıa es de 1h (baterıa estandar de carga a 1C). Es muy molesto tener que cambiarlas baterıas tan frecuentemente, por ello se busco una alternativa para poder trabajar sininterrupciones pudiendo conectar el quadrotor a la red electrica.

Una manera muy economica de hacerlo es reciclar una fuente ATX de PC. Proporcionansalidas de 3.3 V 5 V y 12 V. Para este proyecto se usara el conector de 12 V (cable amarilloy negro de los conectores de 4 pines). A traves de un cable electrico se puede alargar la


Figura 3.11: Fuente ATX

toma para brindar al quadrotor de mas libertad. La fuente ha de permanecer en el suelode manera que solo sea necesario levantar el peso del cable.

Para encender la fuente se debe puentear el cable verde con un cable negro. Si todo haido bien el ventilador debe encenderse.

Figura 3.12: Puente para encender la fuente ATX


3.1.6. Esquema y fotografıa de la plataforma

Figura 3.13: Esquema de la interconexion de subsistemas y fotografıa de la plataforma


3.2. Modificacion y programacion del firmware de la unidadde vuelo

El firmware que viene de serie no provee de las suficientes funcionalidades para estable-cer una interfaz con el PC lo suficientemente rapida por lo que se tuvo que crear un nuevofirmware a medida.

3.2.1. Descarga del codigo fuente del firmware de la Flight Control

En lugar de empezar desde 0, se descargara el codigo fuente del firmware del repositoriopara modificarlo. Se eliminara la mayor parte pero se conservara el esqueleto. Es impor-tante estudiar antes del codigo para moverse dentro de el con facilidad y saber que partesse pueden modificar y que partes no.

Se puede obtener el codigo del repositorio svn. El login y password estan ligados a lascuentas del foro de Mikrokopter por lo que es necesario crear una cuenta para poderacceder.

Despues se puede descargar haciendo un checkout a http://mikrokopter.de/mikrosvn/FlightCtrl/.

Figura 3.14: Tortoise SVN haciendo un checkout al repositorio de Mikrokopter

Se ha utilizado Tortoise SVN para descargar el codigo. Tambien se puede consultar online.Basta con buscar en Google ”Mikrokopter SVN”.

Nota: Es muy recomendable utilizar la rama de Killagreg. El codigo es muy legible yademas esta en ingles en lugar de aleman


3.2.2. Instalacion del compilador y el gestor de Makefiles

La compilacion del codigo se gestiona mediante Makefiles. Estos archivos contienen elprocedimiento que hay que seguir para compilar el codigo y enlazar los distintos archivosobjeto que genera el compilador. Se interpretan con el comando make.

Hay varias opciones para usar make:

1. Linux. El uso de make es muy comun en Linux por lo que es relativamente facil.

2. Windows usando Cygwin. Cygwin proporciona una capa de emulacion de la API deLinux que permite recompilar facilmente programas de Linux para Windows. Entreel set de programas que proporciona se puede encontrar make.

3. make 3.81. Es la que se utiliza en el proyecto actualmente. Hay varias versiones demake para Windows pero no todas funcionan correctamente por lo que se estuvodurante un tiempo haciendo uso de las dos opciones anteriores. No obstante estaversion funciona muy bien con Windows 7 .

Como compilador GCC para AVR se puede utilizar WINAVR. La version utilizada en esteproyecto ha sido WINAVR-20100110.

Se debe editar el Makefile para que las rutas apunten a donde se ha instalado el compi-lador

3.2.3. Compilacion del codigo en Windows y programacion del mi-crocontrolador

Una vez se ha descargado el codigo y se han hecho las modificaciones oportunas hayque compilar y enlazar el codigo. El archivo Makefile contiene la informacion necesariapara hacerlo. Finalmente se flasheara en el microcontrolador.

Para compilar el codigo debemos:

1. Abrir un terminal. Inicio-Ejecutar-cmd.exe

2. Dirigirse al directorio donde se ha guardado el codigo de la Fligth Control. Esto sehara con la orden cd. Por ejemplo

cd C:\FlightCtrl\branches\codigo_modificado

3. Ejecutar make.

4. Si todo ha ido bien el codigo deberıa compilar sin errores. El fichero para flasheares el .hex. El resultado se puede ver en la Figura 3.15


Figura 3.15: Codigo de la Flight Control compilado con exito usando make 3.81 y WINAVR-20100110

Para cargar el firmware en el microcontrolador se usara el MK-TOOL. Hay que dirigirse ala opcion Update Software que aparece tras pulsar el boton Controller Update & Terminalcomo muestra la Figura 3.16. La herramienta de Mikrokopter conectara con el bootloaderinstalado en el microcontrolador y lo programara.

Si el sistema no es capaz de conectar con el microcontrolador se debe revisar la conexiondel puerto serie y resetear el micro. Si el microcontrolador utiliza otra velocidad de puertoserie diferente de 57,6 Kbps se debera hacer un reset manual mientras salga Connect-ing.... El reset provocara que salte el bootloader y se autoconfigure a 57,6 Kbps recono-ciendo el comando de programacion.

Figura 3.16: Programando el microcontrolador usando el MK-Tool


3.2.4. Comunicacion del quadrotor con el PC

Con el nuevo firmware ya se pueden leer los datos a una velocidad suficiente como paraestabilizar el quadrotor. No obstante aun queda por establecer la comunicacion entre elquadrotor y el PC de manera que ambos se entiendan. La placa Flight Control utiliza unprotocolo especial. El nuevo firmware utiliza rutinas del firmware original a efectos de reuti-lizar codigo. Por otra parte, al tener que transmitir informacion por buses de comunicacionexternos aparecen unos problemas que deben tenerse en cuenta.

3.2.4.1. Protocolo de la Flight Control

Las funciones se comunicacion que se encuentran en el codigo fuente siguen este proto-colo:

1. Byte de sincronizacion ’#’: Todos los mensajes empiezan con este caracter.

2. Direccion de origen/destino ’b’: Normalmente se utiliza la b para indicar que lastransmisiones van de/hacia la Flight Control

3. Datos: Los datos vienen codificados en base64 en parte para evitar que aparez-ca el caracter de sincronizacion dentro de los datos.Dentro de los datos viene elidentificador de comando.

4. CRC: Codigo de Redundancia Cıclica para verificar la validez de los bytes.

5. Retorno de carro: Para indicar el fin de la transmision.

En el SVN se pueden encontrar funciones para gestionar el protocolo en Labview bajo larama Projects/GroundStation proporcionadas por el usuario Speedy.

3.2.4.2. Medicion de la latencia de la plataforma

Se puede definir la latencia del sistema como el tiempo que tarda un paquete de datos enir y volver.

Si la latencia es demasiado grande el sistema no sera capaz de responder a tiempo yno podra estabilizarse. Para poder medirla se programaron funciones de ping-pong en elmicrocontrolador y seguidamente se uso el siguiente procedimiento:

1. El PC inicia una solicitud de ping

2. El microcontrolador recibe la solicitud y guarda el estado del reloj del sistema y loenvıa al PC

3. El PC recibe en la contestacion el estado del reloj del sistema y la devuelve sinprocesar


4. El microcontrolador recibe el paquete con el estado del reloj del sistema y a partir delnuevo estado del reloj calcula los milisegundos que ha tardado en volver el paquete

El sistema operativo puede incrementar virtualmente esta latencia reteniendo los datos.No se puede desactivar totalmente pero se puede reducir de 16 ms que viene por defectoa 1 ms. Para ello se debe ir a Administrador de dispositivos y buscar el puerto COM sobreel que se esta trabajando.

Figura 3.17: Reduccion de la latencia virtual que introduce el sistema operativo

3.2.4.3. Velocidad del puerto serie y Full Duplex

Pese a que el conversor serie-usb es full-duplex las pruebas demostraron que fuera develocidades estandar no se comporta como tal. Para certificarlo se desactivo el receptorde la UART en el microcontrolador y se comprobo que la tasa de recepcion de datos noera proporcional al incremento de velocidad. Por ello finalmente se decidio usar 115,2Kbps en lugar de 500 Kbps que es la maxima velocidad de trasferencia que soporta elmicrocontrolador.

Tambien se llego a la conclusion que al funcionar en conmutacion (half-duplex) la latenciaera mas variable que trabajando en full-duplex.

3.2.4.4. Estudio sobre la posibilidad de incorporar un modulo RF

Los modulos RF pueden verse afectados por la propagacion multicamino. Esto ademaspuede verse agravado porque muchos modulos necesitan paquetizar la informacion antesde enviarla. Paquetizar la informacion implica tener que llenar un buffer antes de enviar la


informacion, es decir un tiempo de espera. En el de una red Wifi, la informacion no viajasiempre por el mismo camino por ello la latencia variara dependiendo del estado de la red.

Ante las dudas de sobre como podrıa a afectar se hizo un pequeno estudio que con-sistio en:

1. observar el comportamiento del quadrotor cuando este era sometido a una latenciafija y una variable.

2. ver la latencia que presentan dos modulos Xbee

Para retrasar la informacion un ciclo de reloj en electronica se utilizan biestables. La man-era de hacer esto en Labview es mediante un feedback node. Ası por cada feedback nodeque se ponga se estara retrasando la informacion a razon de un medio de la latencia ha-bitual. Estos se colocaran justo antes de enviar los datos con la potencia de los motores.

Figura 3.18: Incremento de la latencia del sistema por medio de feedback nodes

Los resultados demostraron que el sistema se mantenıa estable siempre y cuando sereajustaran los parametros los PID. Contra mas se variaba la latencia mas habıa que variarlos parametros del PID. Tambien se observo un comportamiento mas ”perezoso.a medidaque la latencia iba creciendo. Como consecuencia se deduce que la latencia variableafecta muy negativamente a la estabilidad del sistema debido a que el sistema no va a iradaptando los parametros del PID dinamicamente y por tanto este se volvera inestable.Durante las pruebas de vuelo con latencia variable el quadrotor mostro un comportamientoviolento e inestable.

Para poder comprobar la latencia de los modulos RF se utilizaron dos Xbee PRO conec-tados a un adaptador USB

Figura 3.19: Adaptador Xbee USB

Se hicieron dos programas en Labview implementado la funcionalidad de ping. Un clientey un servidor. El servidor tan solo debıa devolver lo mismo que enviaba el cliente (untimestamp). El cliente representaba una diferencia de timestamps que correspondıa a lalatencia.

El pico que se aprecia en la Figura 3.21 puede ser suficiente para llevar al quadrotor fuerade la zona de estabilidad. Una vez fuera de esta zona es difıcil recuperarlo. Entrara enuna oscilacion creciente hasta que finalmente se estrellara.


Figura 3.20: Cliente ping hecho en Labview para medir la latencia de los dos modulos

Figura 3.21: Latencia de la comunicacion entre dos modulos zigbee

Otra causa del aumento de latencia brusco puede ser el PC. Si esta muy sobrecargadoes posible que Labview no tenga tiempo de CPU suficiente para procesar y tarde mas delo esperado en generar el resultado.

Tras pensar en formas de eliminar los cables que lo limitan se llego a la conclusion quela solucion mas inmediata es embarcar el ordenador en el quadrotor. En el mercado ex-isten placas de PC a partir de 12cm x 12cm . No obstante se debe brindar la proteccionsuficiente pues al ser un sistema de prototipos las posibilidades de estrellarlo son altas.

Figura 3.22: Placa Nano ITX. Las dimensiones son perfectas para llevar en un quadrotor


3.3. Programacion del conjunto

3.3.1. Consideraciones al trabajar con varias plataformas

3.3.1.1. Endianness

El endianness se refiere a como se guardan las palabras en memoria en arquitecturas demas de 8 bits (suponiendo 8 bits como elemento atomico).

Figura 3.23: Big Endian vs Little Endian

Como muestra la Figura 3.23 si se pasaran los datos de la memoria de una maquina aotra cada una verıa numeros distintos aun siendo el mismo numero. Las dos maquinastienen el mismo numero pero lo guardan y leen de manera distinta. Es por ello que sedebe tener en cuenta y reordenar los bytes. Puede ser una tarea sencilla cuando se tratade arrays donde cada elemento tiene el mismo ancho de bits, es decir un array de 16bits tendra todos los campos de 16 bits y la ordenacion sera tan simple como hacer unintercambio de bytes cada 2 bytes.

Figura 3.24: Intercambio de bytes en un array con ancho de palabra de 16 bits

Pero la cosa se puede complicar cuando hay estructuras que albergan elementos de difer-entes tamanos ya que la ordenacion de los bytes de cada elemento se debe hacer porseparado.


3.3.1.2. Alineacion de memoria

Aun trabajando con la misma arquitectura puede ser que el sistema operativo trabaje demanera diferente. Es el caso de la alineacion de memoria. Algunos sistemas operativoscomo Linux por temas de eficiencia utilizan alineamiento a 32 bits. Es decir todas lasestructuras deberan ser multiplos de 32 bits de lo contrario se rellenaran los bytes quefalten hasta que el tamano de la estructura sea un multiplo de 32 bits.

Figura 3.25: Padding para que el tamano de la estructura sea multiplo de 32 bit (Linux)

Por ejemplo: supongamos que se tiene una estructura que ocupa 5 bytes. Para llegar aser multiplo de 32 bits (alineamiento a 4 bytes) deberıa ocupar 8 bytes. Los 3 bytes quefaltan para completar se rellenan aunque no se utilicen. Es decir en realidad la estructuraestara ocupando 8 bytes como muestra la Figura 3.25

La plataforma ha sido disenada en Windows que utiliza alineacion a 1 byte por lo quesi se quiere utilizar en Linux o Mac se tendran que modificar las funciones de copia deestructuras en los bloques de recepcion de datos.

3.4. Detalles de implementacion

3.4.0.3. Especificaciones del firmware disenado

Como ya se menciono anteriormente el firmware que viene de serie no es capaz de satis-facer las necesidades que presenta este proyecto. Por ello pese a conservar el esqueletose han anadido cosas y se han quitado otras muchas que no se van a utilizar como porejemplo funcionalidades de vuelo.

Las caracterısticas principales son:

1. velocidad de trabajo incrementada a 115,2 Kbps en lugar de 57,6 Kbps para satis-facer a tiempo la demanda de datos hacia el ordenador.

2. periodo de trabajo de 7 ms. Permite un frecuencia de operacion maxima de 143Hzcon toda la electronica de abordo funcionando.

3. soporte para GPS: despues de un reset actua de manera transparente como siestuviera conectado directamente al PC. Esto permite hacer las configuracionesoportunas desde el software ublox center.

4. calculo de latencia en tiempo real.


5. soporte para brujula MK3MAG utilizando el protocolo I2C.

6. soporte para sonar pwm gracias a liberacion del puerto cambiando el protocolo dela brujula.

7. soporte de control de potencia de motores.

Los comandos que acepta siguiendo el protocolo de la Flight Control son:

1. ’6’ : Calibrar la brujula.

2. ’5’ : Desactivar la transmision directa de datos GPS y empezar la transmision contınuade datos de la electronica de abordo cada 7 ms.

3. ’4’: Enviar informacion de los 4 motores. Junto con los motores se envıa el times-tamp de la ultima muestra recibida para calcular la nueva latencia .

4. ’r’: Encender/Apagar el led rojo. Util para depurar.

Y los comandos que envıa:

1. ’I’ : Paquete de datos con las medidas de la electronica de abordo. Se ha de enviarun comando ’5’ previamente.

3.4.0.4. Modelo de operacion

Por sencillez el codigo se implementara de manera que las operaciones de lectura delpuerto serie bloqueen el programa. De esta manera la velocidad de ejecucion del pro-grama se regulara segun el flujo de entrada de datos. Es decir cada vez que se solicitendatos, si no hay, el programa quedara a la espera hasta que lleguen y posteriormenteprocesara los nuevos datos adquiridos.

Este metodo funciona bien para programas que no tengan mucha carga computacional.En otro caso se desperdicia mucho tiempo de CPU durante el bloqueo.

En la Figura 3.26 muestra el esqueleto software de un quadrotor.

Figura 3.26: Esqueleto en Labview para programar quadrotores

El primer paso consiste el abrir el puerto serie para trabajar a un velocidad de 115,2 Kbps(Figura 3.27)


Figura 3.27: Apertura y configuracion del puerto serie en Labview

Figura 3.28: Envıo del comando ’5’ al firmware

El siguiente paso consiste en enviarle el comando ’5’ para que empiece a enviar los datosde los sensores de forma periodica cada 7 ms(Figura 3.28).

El ultimo paso es la creacion del bloque principal. Este bucle se ejecuta cada 7 ms (deter-minado por el firmware) gracias al funcion ”MK READ DATA”. Esta funcion ha sido creadaa medida para comunicarse con el firmware de forma optima. Si no hay datos, el programase para aquı a la espera de que lleguen mas. Esta funcion facilita los datos a traves deun cluster que permite obtener informacion de todos los sensores de abordo ademas delGPS. Es aquı donde se tiene que disenar todo el sistema.

Figura 3.29: Bucle principal

Los datos de los motores se envıan a traves del array rellenado con ceros. Del ”MK READDATA”se extrae tambien el timestamp para calcular la nueva latencia. Una vez todo empa-quetado se envıa de vuelto al quadrotor utilizando el comando ’4’.

Ampliacion de hardware 41

CAPITULO 4. AMPLIACION DE HARDWARE

4.1. Placa Navigation Control

Figura 4.1: NaviCtrl con MKGS y MK3MAG

Mikrokopter provee de una placa de expansion llamada NaviCtrl que amplia las funcional-idades de la Flight Control. A traves del GPS permite anadir funcionalidades como ”vueltaa casa.o ”mantener posicion”. Para ello utiliza un microcontrolador de 32 bit para resolverlas ecuaciones de navegacion y liberar de carga computacional a la Flight Control. A travesde un puerto SPI se transfiere la informacion calculada de la NaviCtrl a la Flight Control.

En este proyecto se estuvo barajando la posibilidad de usar la placa pero finalmente trasnumerosas pruebas se llego a la conclusion que penalizaba sobre la latencia. Finalmentese decidio conectar el resto de la electronica a la Flight Control usando una placa expan-sora disenada para tal fin cuyo esquema esta en la Figura 4.2 y su montaje se puede veren la Figura 4.3

Figura 4.2: Esquema de la placa adaptadora


Figura 4.3: Placa adaptadora montada

4.2. GPS

4.2.1. Fragmentacion para transportar los datos del GPS

Al incorporar informacion GPS empiezan a aparecer problemas. La latencia pasa de serfija a ser variable con un patron que se va repitiendo a lo largo de la grafica (Figura4.4). El quadrotor hace eco de esta situacion y se comporta de manera nerviosa. Esdifıcil mantenerlo en el aire y una pequena perturbacion lo hace reaccionar violentamentesobreactuando y estrellandolo contra el suelo (o el mobiliario en el caso de vuelo interior).

Figura 4.4: Al anadir informacion GPS la latencia pasa de ser casi constante a ser variable

Los datos del GPS tienen una baja frecuencia de actualizacion con la contra que el pa-quete de informacion es muy grande. En un inicio el periodo de muestreo del firmware era


cada 4 ms. La informacion de un bloque GPS tenıa una duracion teorica de 11.7 ms. Enla Figura 4.5 aparece esquematizado lo que sucedıa.

Figura 4.5: Problema al anadir informacion GPS

Despues de transmitir un paquete de datos (IMU DATA) se comprobaba si habıan datosGPS para transferir y si los habıan se enviaban justo despues. El caso es que el tiempodisponible para el envıo antes de que se enviara el siguiente bloque de datos de sensoresera de 1.3 ms. Un bloque de GPS tardaba en enviarse 11.7 ms. Durante este tiempo nose podıan enviar datos de los sensores.

Ante esta situacion podemos optar entre 3 soluciones:

1. Aumentar el periodo de muestreo: El periodo total de muestreo deberıa ser cada 13ms.

2. Fragmentar los datos: Los datos del GPS se deben fragmentar de manera que detiempo a transmitirse en el tiempo que hay disponible justo despues de los datos(zona pintada de verde).

3. Una combinacion de las dos anteriores: Aumentar el tiempo de muestreo a la vezque se hace uso de la fragmentacion de datos.

Finalmente tras varias pruebas se decidio aumentar el periodo de muestreo a 7 ms yfragmentar los datos del GPS en bloques de 8 bytes. El total de la informacion de laestructura se recibıa al cabo de 15 ciclos. Se tuvo que modificar el firmware para tal finaumentando la complejidad del mismo. Las pruebas de vuelo efectuadas no presentaronincidencias salvo que se tuvieron que modificar los parametros de los PIDs debido alcambio de la latencia.

4.3. Brujula

La Flight Control tiene implementada la lectura de la brujula a traves de PWM. No obstanteeste puerto era necesario para el sonar por lo que se tuvo que cambiar el protocolo de labrujula e implementar la lectura a partir del I2C.


4.3.1. Protocolo de comunicacion

EL problema de la brujula de Mikrokopter es que no tiene especificaciones por lo que paraentender el protocolo de comunicacion se debe recurrir al codigo fuente del repositorio.Tras varios analisis del codigo y pruebas se determino la secuencia de lectura y escritura.

Para leer de la brujula:

1. Generar START

2. Escribir 50h (direccion I2C escritura de la brujula)

3. Escribir comando

4. Generar START

5. Escribir 51h (direccion I2C lectura de la brujula)

6. Leer

7. Leer CRC

8. Generar Stop

Para escribir en la brujula

1. Generar START

2. Escribir 50h (direccion I2C escritura de la brujula)

3. Escribir comando

4. Escribir datos

5. Escribir CRC

6. Generar Stop

4.4. Sonar EZ0

El sonar EZ0 es capaz de medir distancias por ultrasonidos y proporcionar la lectura atraves de varios puertos. Se escogio el puerto PWM para la lectura de datos.


4.4.1. Modelo de eliminacion de falsas lecturas

Al funcionar por ultrasonidos es probable que el sonar obtenga falsas lecturas debido arebotes con el medio. Para evitar esto se implemento un sistema antirebotes. Su principiode funcionamiento es muy simple: la muestra actual menos la anterior en valor absolutono debe sobrepasar un umbral. En caso de sobrepasarlo se usa la ultima lectura. El filtropaso bajo se utiliza para suavizar la senal.

Figura 4.6: Eliminacion de falsas lecturas del sonar


Conclusiones 47

CAPITULO 5. CONCLUSIONES

El sistema cumple todos los objetivos planteados. Con el software y los algoritmos de con-trol implementados es capaz de volar en interiores en control manual y realizar cambiosde rumbo bruscos sin perder la estabilidad. Ademas el software desarrollado es facilmenteescalable y se adapta muy facilmente a otras plataformas de quadrotores distintas que laque se ha utilizado (Mikrocopoter)

Pese a que se han cumplido todos los objetivos, la dificultad ha sido mucho mayor de laprevista debido a la multidisciplinaridad del proyecto. Muchas de las soluciones planteadasen este escrito son solo una de multiples que se han probado.

En un futuro se desea perfeccionar el quadrotor, completar el software de navegacion GPSası como dotar al sistema de vision artificial. Si es posible tambien se intentara presentarloa algun concurso de ambito internacional. Para esto ultimo se ha de “convertir” el sistemadisenado en un sistema embebido en el propio hardware de la plataforma.


BIBLIOGRAFIA 49

BIBLIOGRAFIA

[1] Jose Julio. “ArduImu Quadcopter”. http://diydrones.com/profiles/blogs/arduimu-quadcopter

[2] Premerlani-William and Bizard-Paul. “DCM IMU:Theory” .http://gentlenav.googlecode.com/files/DCMDraft2.pdf

[3] “Mikrokopter” . http://www.mikrokopter.de

[4] “Diydrones” . http://www.diydrones.com

Documents

Interfície LabView per programar el sistema de control de quadrotors