Sistema de seguimiento tridimensional basado en un array de sensores de flujo óptico implementados con cámaras de baja resolución

Trabajo de Fin de M´aster ”M´aster Universitario en Microelect´onica: Dise˜no y Aplicaciones de Sistemas Micro/Nanom´etricos” Sistema de seguimiento tridimensional basado en un array de sensores de flujo ´optico implementados con c´amaras de baja resoluci´on Autor: Rafael de la Rosa Vidal Tutor: Juan Antonio Le˜nero Bardallo 30 Noviembre 2020 A mi familia A mis profesores i ii Agradecimientos Despu´es de un a˜no de trabajo y mucho esfuerzo, por el hecho de combinar la vida laboral con estudio superiores, me dispongo a cerrar un nuevo cap´ıtulo en mi carrera acad´emica. Desde el principio he estado rodeado de personas que me han aportado el conocimiento y el apoyo necesarios para alcanzar todas las metas que me he propuesto. A todos ellos quiero darles las gracias. En primer lugar, me gustar´ıa agradecerles a todos los profesores y profesionales que han construido las bases de mi formaci´on por su dedicaci´on y tes´on. En especial a mi tutor, Juan Antonio Le˜nero, qui´en ha puesto a mi disposici´on todo el conocimiento y las herramientas necesarias para la ejecuci´on de este trabajo as´ı como una total confianza en m´ı. A Jos´e Mar´ıa Guerrero, a quien le tengo mucho aprecio y respeto, que me ha brindado oportunidades de crecimiento profesional por las cuales siempre le estar´e inmensamente agradecido y que, indirectamente, tambi´en es parte de este proyecto. A Jes´us ´ Alvarez, un gran profesional que me ha ense˜nado una buena parte de lo que en este trabajo se refleja y, que es una gran persona a la que le tengo mucha estima, siempre me ha servido de apoyo y motivaci´on. A mi familia, un pilar fundamental en mi vida, por apoyarme en todas mis decisiones y por hacer de m´ı la persona que soy a d´ıa de hoy. A mi pareja, por su apoyo incondicional, por soportarme y por ser mi principal fuente de motivaci´on cada vez que algo se tuerce. Siempre os estar´e eternamente agradecido. iii iv Resumen En este Trabajo Fin de M´aster se ha dise˜nado, implementado y testado un sistema de seguimiento tridimensional basado en un array de sensores de Flujo ´ Optico procesando im´agenes captadas con c´amaras de baja resoluci´on. Se trata de un proyecto multidisciplinar en el que se han abarcado tareas de dise˜no mec´anico, dise˜no hardware, desarrollo firmware y software. El objetivo global del proyecto ha sido construir una plataforma m´ovil que permite posicionar arrays de sensores de imagen para seguir objetos m´oviles mediante el c´omputo del Flujo ´ Optico minimizando el coste computacional. El sistema de seguimiento debe ser vers´atil y poderse adaptar a otros algoritmos y tipos de sensores. La elecci´on de c´amaras de baja resoluci´on para el dise˜no de los sensores de Flujo ´ Optico permite aliviar los requisitos computacionales para su procesamiento, siendo la informaci´on contenida en las im´agenes suficiente para tal fin. Por otro lado, el algoritmo utilizado presenta un menor grado de requisitos computacionales en comparaci´on con el estado del arte. La implementaci´on de todo el firmware desarrollado se ha orientado al aumento del rendimiento del sistema, para ello se ejecutan estrategias que eliminan los tiempos de holgura entre la adquisici´on de los datos y su procesamiento. Se consigue alcanzar una tasa de adquisici´on m´as procesamiento de 5 FPS para cada sensor del sistema, lo que para un total de dos sensores equivale a 10 FPS. La metodolog´ıa del proyecto ha sido la siguiente: se parte del dise˜no de una PCB y puesta en marcha de los algoritmos de Flujo ´ Optico con orientaci´on al rendimiento computacional. Luego se hace el dise˜no mec´anico y modelado 3D de todos los elementos del sistema, dando lugar a un modelo virtual 3D del sistema completo. Se fabrican las piezas necesarias para la fabricaci´on de la plataforma y, finalmente, se llevan a cabo tareas de configuraci´on y control autom´atico. Con el fin de alcanzar la funci´on de seguimiento deseada, se dise˜na una interfaz de usuario que permite modificar el comportamiento de la plataforma, as´ı como la monitorizaci´on de todos los datos generados por esta. A partir de estos datos, se confecciona una ley de control que vincula el valor de Flujo ´ Optico devuelto por los m´odulos EyesOF con el valor de las se˜nales de control de posici´on de la plataforma. El sistema implementado es una plataforma rob´otica vers´atil ´util para implementar y testar algoritmos de seguimiento que requieran el posicionamiento de c´amaras. Puede usarse con sensores y algoritmos de distinta naturaleza a los presentados en el proyecto para resolver problemas diversos que abarquen el posicionamiento y el seguimiento de trayectorias. v vi Abstract In this Master’s thesis, a tridimensional tracking system based on an Optical Flow sensor array was designed and tested. The Optical Flow sensors were implemented with low-resolution cameras. The project is multidisciplinary as it includes mechanical design, hardware design, software development and firmware development tasks. The global project target has been the robotic platform construction which allows to position an image sensor array to track mobile objects employing the Optical Flow computation and maximizing the system performance. The system must be versatile and adaptable for other algorithms and image sensor types. The low-resolution cameras chosen for the Optical Flow sensor design allow alleviating the computational requirements for its processing. The information contained in the low-resolution images is enough for the Optical Flow computation. On the other side, the used algorithm presents a minor grade of computational requirements in comparison with the state-of-the-art. The firmware implementation was oriented to increase the system performance using code strategies which remove slack times between the data acquisition and their processing. Due to this, it has reached an acquisition and processing rate of about 5 FPS for each Optical Flow sensor, as the system has two sensors, the total rate achieved is about 10FPS. The study starts with the PCB design and performance-oriented Optical Flow algorithm settingup. Then it is done the mechanical design and 3D model of all system elements, giving rise to a 3D virtual model of the complete system. Some parts of the platform were made as the platform construction need them. Finally, a set of tasks about setup and automatic control have been accomplished. To implement the desired mobile object tracking function, it was developed a desktop application which makes possible to change the platform behaviour as well as the monitoring of all the data generated by the platform. From the gathered data, it was composed a control law which relates the control signals and the platform position. The developed system is a versatile robotic platform useful for algorithms about tracking function implementation and their testing which requires the camera positioning. The system can be used with algorithms and sensor types different from the ones presented in this study. A large number of functions about positioning or trajectory tracking are suitable for this platform. vii xiv Lista de abreviaturas MB MegaByte. MEMS Micro-ElectroMechanical Systems. MIPS Millions of Instructions per Processor Second. MISO Master Input Slave Output. MOSFET Metal Oxide Semiconductor Field Effect Transistor. MOSI Master Output Slave Input. MSB Most Significant Bit. MVVM Model-View-ViewModel. OCTAVE Optic flow based Control sysTem for Aerial VEhicles. PC Personal Computer. PCB Printed Circuit Board. PID Proportional-Integral-Derivative (controller). PWM Pulse Width Modulation. RC Radio Control. RPM Revoluciones Por Minuto. SCK Serial Clock. SDIO Serial Data Input Output. SOF Start Of Frame. SPI Serial Peripheral Interface. STL Standard Triangle Language. TPI Threads Per Inch. UAV Unmanned Aerial Vehicle. USART Universal Synchronous/Asynchronous Receiver Transmitter. USB Universal Serial Bus. WPF Windows Presentation Foundation. XAML Extensible Application Markup Language. Cap´ıtulo 1 Introducci´on y motivaci´on En la naturaleza existen multitud de organismos relativamente simples que realizan tareas rutinarias complejas de manera eficaz y eficiente (volar, nadar, evitar objetos en vuelo, etc.). A lo largo de la historia muchos de los avances que han desembocado en el desarrollo de tecnolog´ıas actuales se han basado en la observaci´on de comportamientos biol´ogicos existentes en la naturaleza. Se pueden enumerar distintos ejemplos como la capacidad de volar de las aves y la aeron´autica, el sistema de orientaci´on de los murci´elagos y el sistema radar por ultrasonidos, entre otros. Hoy en d´ıa, se contin´ua estudiando los sistemas biol´ogicos como referencia para nuevos dise˜nos, se intenta imitar el comportamiento de los seres vivos mediante su traducci´on a tecnolog´ıas ya desarrolladas, dando lugar a sistemas “bioinspirados” [1, 2, 3, 4]. Existen varios ejemplos de esta tendencia como la empresa Festo ® [5], que en su rama “Bionics Learning Network” lleva desarrollando sistemas rob´oticos imitando el comportamiento de multitud de seres vivos desde 2006; o la iniciativa europea Human Brain Project cuyo objetivo es poner en marcha una infraestructura de investigaci´on que permita a los investigadores cient´ıficos e industriales avanzar en el conocimiento en los campos de la neurociencia, la inform´atica y la medicina relacionada con el cerebro [6]. La “Inteligencia Artificial”, una materia muy popular en la actualidad, trata precisamente de desarrollar un comportamiento basado en el funcionamiento del sistema nervioso humano. Tecnolog´ıas como el reconocimiento de voz o el reconocimiento de im´agenes tienen como objetivo imitar las funciones que realiza un humano en su vida diaria y poder dotar a una m´aquina de dichas funciones. El “Machine Learning” [7] o el “Deep Learning” [8] tratan de implementar el mecanismo que posee el cerebro humano para modificar su plasticidad sin´apitca en funci´on de los est´ımulos que procesa, es decir, emular el aprendizaje. Son tecnolog´ıas que derivan de la observaci´on y el estudio del comportamiento del cerebro humano. Uno de los campos de mayor inter´es es la percepci´on visual que tienen diferentes seres vivos del mundo real y la interacci´on entre ellos [9]. Insectos como las hormigas o las abejas realizan tareas de manera coordinada cuya ejecuci´on no les resultar´ıa posible si no existiera dicha coordinaci´on. Por otro lado, sus habilidades de navegaci´on a´erea est´an muy desarrolladas y poseen una alta capacidad de evasi´on de obst´aculos a pesar de que su organismo no es especialmente complejo comparado con otros seres vivos. Esto ´ultimo, hace que los insectos sean sujetos muy adecuados para el estudio sus capacidades, sus estructuras fisiol´ogicas son m´as simples y encontrar las relaciones est´ımulo-acci´on es m´as sencillo. En este trabajo se va a desarrollar el concepto de “Flujo ´optico” cuyo origen se encuentra precisamente en el estudio de la percepci´on del mundo real de los seres vivos [10]. El Flujo ´optico permite a los seres vivos navegar por el espacio tridimensional, tener consciencia de los movimientos que tienen los objetos que les rodea respecto a su punto de vista. 1 2 1.1. MOTIVACIONES Y OBJETIVOS 1.1. Motivaciones y objetivos Muchos autores, que se revisar´an en el Cap´ıtulo 2, han trabajado en el c´omputo del Flujo ´ Optico, desarrollando sistemas que consiguen extraer el Flujo ´optico del espacio que rodea al sistema. La mayor´ıa de ellos requieren de una capacidad computacional muy elevada lo que limita su implementaci´on a aplicaciones que posean un suministro de energ´ıa no limitado. Otros, aunque no necesitan de tal capacidad computacional, s´ı que hacen uso de un hardware muy espec´ıfico, y, se centran en caracterizar dicho hardware, no en hacer uso de la informaci´on generada. Por otro lado, ambos enfoques aumentan el coste econ´omico del sistema, los sistemas con mayor capacidad computacional son m´as caros, al igual que la fabricaci´on de hardware muy espec´ıfico. En este trabajo se pretende desarrollar un sensor de Flujo ´optico que no presente requisitos computacionales elevados para su implementaci´on y que se encuentre constituido por componentes comerciales. Una vez desarrollado, utilizar la informaci´on de dos de estos sensores acoplados a una plataforma m´ovil para implementar el seguimiento de objetivos. De esta manera, con tal fin, los objetivos de este Trabajo Fin de M´aster fueron los siguientes: Estudiar el estado del arte de sistemas relacionados, as´ı como de las distintas opciones disponibles para la implementaci´on de la estimaci´on de Flujo ´ Optico. Analizar algunos de los algoritmos extra´ıdos del estado del arte para comprobar cu´al es el m´as adecuado para la aplicaci´on. Dise˜nar, desarrollar y justificar el hardware y el firmware necesarios para obtener un sistema de dos sensores de Flujo ´ Optico operativo. Dise˜nar, modelar y construir la plataforma mec´anica que permita el movimiento de los sensores en el espacio tridimensional. Estudiar la din´amica de la plataforma desarrollada e implementar el control necesario para su estabilizaci´on. Dise˜nar una aplicaci´on de escritorio que permita monitorizar los datos generados por la plataforma construida, as´ı como su ajuste, operaci´on y control. Analizar la respuesta din´amica del sistema y, en base a esta informaci´on, ajustar los par´ametros de control precisos para la aplicaci´on de seguimiento de objetivos. A tal efecto, este estudio se organiza de la siguiente forma: el Cap´ıtulo 2 contiene el estado del arte de sistemas similares, as´ı como de los modelos e implementaciones para la estimaci´on del Flujo ´ Optico. El Cap´ıtulo 3 introduce aspectos te´oricos del Flujo ´ Optico, su formulaci´on, as´ı como el an´alisis de algunos de los algoritmos m´as relevantes. El Cap´ıtulo 4 desarrolla el proceso de dise˜no del hardware y el firmware que forma el sistema constituido por dos sensores de Flujo ´ Optico. Se manda a fabricar el dise˜no, se sueldan todos sus componentes y se realizan diferentes test para su puesta en marcha. Basados en el dise˜no de estos m´odulos se confeccionar´a una plataforma rob´otica donde ser´a acoplados los sensores de Flujo ´ Optico. El Cap´ıtulo 5 contiene el estudio de los componentes usados en la plataforma, su dise˜no mec´anico y el ajuste de los par´ametros de control relacionados a partir del estudio de su din´amica. Se realiza un modelo 3D detallado y a escala de cada elemento del sistema y son dispuestos en el espacio de forma coherente dando lugar a una construcci´on virtual de la plataforma al completo. Esto permite evitar problemas de colisiones entre elementos en fases de construcci´on posteriores y ajustar las dimensiones de algunos elementos relevantes para cumplir con algunas restricciones mec´anicas necesarias para el correcto funcionamiento del sistema. Varios elementos del sistema precisan de su fabricaci´on para la posterior construcci´on de la plataforma, para ello se han exportado los modelos 3D correspondientes a un formato compatible CAP´ ITULO 1. INTRODUCCI´ ON Y MOTIVACI´ ON 3 con m´etodos de fabricaci´on aditiva, como la impresi´on 3D. Se ha tenido en cuenta la tolerancia del proceso en el dise˜no de dichos elementos y han sido fabricados en las impresoras 3D disponibles en el IMSE. Tras la construcci´on del sistema se han realizado tareas de configuraci´on y control autom´atico (estudios de la din´amica del sistema, ajustes de reguladores PID, etc.) con el objetivo de estabilizar la plataforma. Adem´as, se implementan se˜nales de control que permitan modificar la posici´on de la plataforma seg´un se requiera. Por ´ultimo, el Cap´ıtulo 6 recoge todo el proceso de dise˜no de la aplicaci´on de escritorio para la monitorizaci´on, ajuste y control de la plataforma. Se explica la tecnolog´ıa utilizada as´ı como las herramientas usadas para la representaci´on de los datos. En el Cap´ıtulo 7 se hace una revisi´on de los objetivos alcanzados as´ı como de diferentes l´ıneas de trabajo futuras originadas durante el desarrollo del estudio. 4 1.1. MOTIVACIONES Y OBJETIVOS Cap´ıtulo 2 Estado del arte La detecci´on de movimiento a trav´es del estudio de una secuencia de im´agenes es un problema que se lleva estudiando desde hace d´ecadas. Ya en el siglo XIX el cient´ıfico von Helmholtz [11] comenz´o a investigar sobre la idea de la detecci´on de movimiento a trav´es de la informaci´on contenida en una imagen, aunque no aportaba un contenido te´orico. No fue hasta la d´ecada de 1940, cuando el psic´ologo norteamericano Gibson [10] en su trabajo sobre la “Percepci´on Visual del mundo real en seres vivos” plante´o este problema acu˜nando el concepto de “Flujo ´ Optico”. Aunque Gibson [10] no pose´ıa las herramientas necesarias para resolver el problema, dio lugar a muchos estudios que buscaban dar al concepto un sentido matem´atico y te´orico [12, 13, 14, 15, 16]. Otros investigadores se centraron en el sentido biol´ogico, es decir, en el estudio de las estructuras que permit´ıan a los seres vivos detectar el Flujo ´ Optico [17, 18, 19, 20, 21]. El conocimiento aportado por estos trabajos deriv´o en estudios donde el objetivo era la estimaci´on del Flujo ´ Optico mediante la implementaci´on de estructuras que simularan el comportamiento biol´ogico de las estructuras utilizadas por los seres vivos para este fin (sistemas “bioinspirados”) o, por otro lado, la implementaci´on de los modelos te´oricos en sistemas computacionales. El trabajo de Srinivasan [22] estudia la detecci´on de Flujo ´ Optico en los insectos, c´omo utilizan esta informaci´on para navegar por el espacio del mundo real, donde coexisten una serie de factores como obst´aculos y depredadores, y desarrolla una serie de aplicaciones rob´oticas bas´andose en el sistema de percepci´on de los insectos. En Srinivasan [23] y Baird et al. [24] se estudia un modelo de robot basado en el comportamiento de las abejas en tareas de navegaci´on, en particular de la especie Apis mellifera L. Franceschini et al. [25] expusieron el papel de la percepci´on de Flujo ´ Optico en las tareas de despegue, estabilizaci´on de vuelo y aterrizaje en insectos voladores. Estas tareas para los insectos son cotidianas y las realizan sistemas de consumo muy bajo y con muy baja latencia, de ah´ı el atractivo de intentar basar la operaci´on de sistemas rob´oticos en los mismos principios. En Ruffier and Franceschini [26] se desarrolla OCTAVE (“Optic flow based Control sysTem for Aerial VEhicles”), un piloto autom´atico para aeronaves basado en el Flujo ´ Optico detectado mediante un circuito bioinspirado denominado EMD (Elementary Motion Detector) [27] (Figura 2.1) resultado del estudio de grabaciones electrofisiol´ogicas al aplicar microest´ımulos a un ´unico fotorreceptor del ojo de un insecto. Con la llegada de los sistemas computacionales modernos, se comienza a estudiar la posibilidad de implementar mediante algoritmos computacionales modelos te´oricos de estimaci´on de Flujo ´ Optico. En Barron et al. [28] y Baker et al. [29] se muestra una comparaci´on de los resultados obtenidos por diferentes implementaciones de modelos te´oricos de Flujo ´ Optico. La base de la mayor´ıa de los algoritmos son los modelos de Horn and Schunck [13] y Lucas and Kanade [15], aunque cada autor ha realizado diferentes optimizaciones que hacen que el modelo presente un rendimiento optimizado as´ı como resultados m´as precisos. Por ejemplo, algoritmos propuestos por Murray and Buxton [30], Black and Anandan [31] y Brox et al. [32] implementan el modelo de Horn 5 6 Figura 2.1: Implementaci´on de un detector EMD para el sistema OCTAVEExtra´ıdo de [26] and Schunck [13] a˜nadi´endole filtros espacio-temporales, entre otras optimizaciones, que mejoran la precisi´on del Flujo ´ Optico resultante. Otros algoritmos como los propuestos por Jung et al. [33] y Lempitsky et al. [34] utilizan la fusi´on de los dos modelos anteriores. A pesar de las m´ultiples optimizaciones realizadas en el tiempo, aunque la precisi´on de los resultados ha mejorado sustancialmente, los algoritmos de Flujo ´ Optico son muy demandantes computacionalmente y necesitan de m´aquinas potentes para ejecutarlos en im´agenes con una cadencia propias del tiempo real. Estos algoritmos han conducido a la aplicaci´on del Flujo ´ Optico en diferentes ´ambitos como son el diagn´ostico m´edico [35, 36, 37], la vigilancia [38, 39], la producci´on de contenido televisivo [40], el seguimiento de objetivos [41, 42], prevenci´on de incendios [43], entre otros. Cada aplicaci´on utiliza el algoritmo que mejor se comporte frente a las caracter´ısticas de las im´agenes capturadas: ruido, rango din´amico, etc. Muchas de las aplicaciones necesitan de un procesamiento previo de las im´agenes antes de aplicar el algoritmo en cuesti´on, lo que aumenta los requisitos de potencia computacional. En este trabajo el Flujo ´ Optico es utilizado para el seguimiento de objetivos en el espacio tridimensional abordando no s´olo el dise˜no del sensor de Flujo ´ Optico sino que tambi´en se dise˜na la plataforma mec´anica que permitir´a posicionar correctamente los sensores as´ı como la implementaci´on de su control. Este tipo de sistemas han sido implementados por otros autores como, por ejemplo, en Delbruck and Lang [44] se desarrolla un robot portero, mostrado en la Figura 2.2, que detecta las bolas lanzadas hacia ´el y las intercepta. Aunque en este caso no se utilizan algoritmos de Flujo ´ Optico, puesto que se usa un sensor DVS [45] que devuelve los p´ıxeles de la imagen que presentan movimiento, el concepto de controlar la plataforma a partir de la informaci´on visual es parecido al que se acoge en este trabajo. La respuesta de la plataforma es muy r´apida y el consumo de CPU se encuentra por debajo del 4 % pero, ha de anotarse la existencia de un dispositivo de alta capacidad computacional como es un PC. Adem´as, la plataforma solo presenta un grado de libertad en el eje horizontal. En Pericet-Camara et al. [46] se construye una plataforma que permite caracterizar el sensor de Flujo ´ Optico bioinspirado desarrollado en el estudio denominado CURVed Artificial Compound Eyes (CURVACE) [47]. El sensor CURVACE contiene dos microcontroladores de 16-bits de la familia dsPIC33F[48] con una capacidad de 40 MIPS. La plataforma permite un grado de libertad en el eje horizontal para realizar mediciones de Flujo ´ Optico en condiciones de rotaci´on conocidas y, as´ı, poder caracterizar el dispositivo. En la referencia se estudia el comportamiento de dos algoritmos: una versi´on extendida de Lucas and Kanade [15] y el algoritmo de Srinivasan [49]. Este ´ultimo algoritmo es el que se va a desarrollar e implementar en este trabajo. La presencia de elementos basados en la estimaci´on de Flujo ´ Optico en dispositivos rob´oticos, sat´elites, etc. actuales es cada vez m´as frecuente debido al aumento de sus prestaciones, la fiabilidad que ofrecen y la optimizaci´on de los recursos computacionales requeridos. CAP´ ITULO 2. ESTADO DEL ARTE 7 Figura 2.2: Robot portero desarrollado en [44] - Extra´ıdo de [44] Figura 2.3: Sistema desarrollado en [46]: (a) Sensor de Flujo ´ Optico CURVACE (b) Sensor y plataforma m´ovil - Extra´ıdo de [46] 8 Cap´ıtulo 3 Flujo ´optico El Flujo ´ Optico es la distribuci´on del movimiento aparente de patrones de luminosidad en un frame1[13], suponiendo que los cambios en la luminosidad del frame se deben ´unicamente al movimiento de elementos en el frame [50]. Se trata de un movimiento relativo entre distintos objetos y un observador [50, 10, 51], por tanto, esta magnitud puede aportar informaci´on muy valiosa a la hora de localizar y monitorizar la situaci´on de los objetos en un espacio a trav´es de una secuencia de frames separados en el tiempo de dicho espacio [52]. Por otro lado, las discontinuidades en el Flujo ´ Optico permiten realizar tareas de segmentaci´on del frame [53, 54, 55]. La segmentaci´on se basa en la detecci´on de discontinuidades en el Flujo ´ Optico de forma que las zonas donde el Flujo ´ Optico es continuo se relacionan con un mismo objeto en la misma. En la Figura 3.1 se muestra un ejemplo de segmentaci´on mediante Flujo ´ Optico, el bateador es detectado debido a que es la ´unica zona donde existe Flujo ´ Optico puesto que el fondo del frame permanece quieto [56]. La estimaci´on del Flujo ´ Optico es un problema ampliamente analizado por la comunidad cient´ıfica. A pesar del gran n´umero de modelos y algoritmos diferentes, ninguno de estos cubre algunos problemas que aparecen en implementaciones de procesamiento en tiempo real para condiciones reales [57]. El c´alculo de Flujo ´ Optico se basa en dos pasos fundamentalmente [50]: calcular las derivadas espacio-temporales, o lo que es lo mismo, medir las velocidades normales a estructuras locales de intensidad luminosa. combinar las velocidades normales para llegar a las velocidades totales, mediante, por ejemplo, minimizaci´on de m´ınimos cuadrados [15, 49] o regularizaci´on [13]. En los apartados que siguen se va a desarrollar la teor´ıa que existe detr´as del Flujo ´ Optico, algunos algoritmos que implementan el c´alculo de Flujo ´ Optico y sus ventajas e inconvenientes, as´ı como la elecci´on del algoritmo que se ha hecho en este trabajo y su justificaci´on. 3.1. Teor´ıa del c´alculo del Flujo ´ Optico La base para el c´alculo de Flujo ´ Optico es la ecuaci´on de restricci´on de movimiento [50]. Dada la intensidad de un pixel en el centro de un grupo de n×npixeles por la funci´on I(x, y, t), si ´este se mueve una cantidad δx yδy en el plano cartesiano en un tiempo δt, pasa a venir definido por 1Un frame es una matriz bidimensional cuyos elementos (p´ıxeles) representan el valor de la luminancia en una regi´on proporcional a su extensi´on dentro de la escena visual captada por una c´amara. 9 16 3.3. ALGORITMO DE INTERPOLACI´ ON DE SRINIVASAN Figura 3.3: Regiones resultado de un frame de 8 ×8 p´ıxeles y ∆xref = ∆yref = 1 versi´on estimada mediante interpolaci´on de la regi´on f. El problema ahora consiste en obtener los valores c ∆xyc ∆yque minimicen el error entre fyb f. b f=f0+ 0,5 c ∆x ∆xref !(f2−f1)+0,5 c ∆y ∆yref !(f4−f3) (3.23) La minimizaci´on del error entre fyb fse realiza mediante la optimizaci´on de la expresi´on del error cuadr´atico medio estudiado sobre la funci´on ventana Ψ(x, y) que define la regi´on de estudio en el frame. En la Ecuaci´on 3.24 se muestra la expresi´on del error cuadr´atico medio para un frame de M×Np´ıxeles. E= M X x=0 N X y=0 Ψ(x, y)·hf(x, y)−b f(x, y)i2(3.24) Si se sustituye la Ecuaci´on 3.23 en la Ecuaci´on 3.24 se tiene la Ecuaci´on 3.25. Para su minimizaci´on se obtienen las derivadas parciales respecto a c ∆xyc ∆yy se igualan a cero, obteni´endose el sistema de ecuaciones formado por la Ecuaci´on 3.26 y la Ecuaci´on 3.27. E= M X x=0 N X y=0   Ψ·(f−"f0+ 0,5 c ∆x ∆xref !(f2−f1)+0,5 c ∆y ∆yref !(f4−f3)#)2  (3.25) CAP´ ITULO 3. FLUJO ´ OPTICO 17 c ∆x ∆xref !· M X x=0 N X y=0 Ψ·(f2−f1)2 + c ∆y ∆yref !· M X x=0 N X y=0 [Ψ ·(f4−f3)·(f2−f1)] = 2 · M X x=0 N X y=0 [Ψ ·(f−f0)·(f2−f1)] (3.26) c ∆x ∆xref !· M X x=0 N X y=0 [Ψ ·(f2−f1)·(f4−f3)] + c ∆y ∆yref !· M X x=0 N X y=0 Ψ·(f4−f3)2 = 2 · M X x=0 N X y=0 [Ψ ·(f−f0)·(f4−f3)] (3.27) El valor de c ∆xyc ∆yresultado de la resoluci´on del sistema de ecuaciones (Ecuaci´on 3.28 y Ecuaci´on 3.29) es el valor del Flujo ´ Optico para la regi´on Ψ(x, y). c ∆x=C·D−B·E A·D−B2(3.28) c ∆y=A·E−B·C A·D−B2(3.29) donde A= M X x=0 N X y=0 Ψ·(f2−f1)2 B= M X x=0 N X y=0 [Ψ ·(f2−f1)·(f4−f3)] C= M X x=0 N X y=0 [Ψ ·(f−f0)·(f2−f1)] D= M X x=0 N X y=0 Ψ·(f4−f3)2 E= M X x=0 N X y=0 [Ψ ·(f−f0)·(f4−f3)] Existen varias situaciones donde el sistema resulta ser indeterminado: 18 3.3. ALGORITMO DE INTERPOLACI´ ON DE SRINIVASAN El frame es una estructura homog´enea donde no existe cambios en la intensidad de los p´ıxeles. En este caso todos los coeficientes de la soluci´on son nulos debido a que f1=f2=f3=f4. El frame solo contiene caracter´ısticas en una dimensi´on, por ejemplo, una l´ınea horizontal o una l´ınea vertical. En el caso de caracter´ısticas horizontales f2−f1es nulo, mientras que en el caso de caracter´ısticas verticales f4−f3es nulo. En estos dos casos las funciones dejar´ıan de ser independientes y no existir´ıa una soluci´on ´unica. Este problema es el denominado Problema de la Apertura que se comenta en la Secci´on 3.1. Estas situaciones se pueden detectar monitorizando los valores de los distintos coeficientes y as´ı evitar resultados err´oneos. Adem´as, no suelen darse debido a la existencia de ruido en frames reales. En este trabajo se va a utilizar una ventana Ψ(x, y) uniforme que servir´a simplemente para seleccionar una regi´on dentro del frame. Cap´ıtulo 4 Dise˜no hardware y firmware para la adquisici´on del frame y el c´alculo del Flujo ´ Optico Durante este cap´ıtulo se va a desarrollar la fase de dise˜no hardware y firmware necesarios para la adquisici´on del frame y el c´alculo de Flujo ´ Optico. En primer lugar, se va a presentar el sensor de imagen de baja resoluci´on que se va a usar; en funci´on de las capacidades de este sensor se discutir´a el dise˜no hardware necesario para su adaptaci´on al sistema, as´ı como la elecci´on del hardware para la lectura del sensor. En segundo lugar, se presentar´a en detalle el firmware desarrollado para implementar la aplicaci´on, as´ı como las distintas decisiones tomadas en el proceso. 4.1. Sensor ´optico ADNS2610 Los sensores de la familia ADNS son sensores ´opticos orientados a su implementaci´on en ratones ´opticos utilizados como perif´ericos en PCs. Este dispositivo facilita un m´etodo que no precisa de elementos mec´anicos para mover el puntero en la pantalla del PC de acuerdo con el movimiento del sensor integrado en el rat´on de PC. El chip, mostrado en la Figura 4.1, est´a constituido fundamentalmente por un sensor de imagen CMOS de 18 ×18 p´ıxeles y un DSP. Mediante el sensor CMOS se capta la imagen exterior al sensor, que en la aplicaci´on particular de un rat´on de PC ser´a la superficie sobre la que se desliza el rat´on, y en el DSP se procesa el frame adquirido para obtener las caracter´ısticas del movimiento (magnitud, direcci´on y sentido) que se desea [70]. A2610 XYYWWZ SCK SDIO OSC OUT OSC IN LED CONTROL GND VDD REFA Figura 4.1: Chip ADNS2610 y su pinout 19 20 4.2. DESCRIPCI´ ON DE LA ARQUITECTURA HARDWARE DEL DISPOSITIVO Figura 4.2: Descripci´on de los bits en el registro PixelData Todos los par´ametros de configuraci´on del sensor, el frame adquirido, as´ı como las medidas realizadas se encuentran disponibles en los distintos registros internos del chip a trav´es de una interfaz serie de dos hilos. Dicha interfaz est´a compuesta por una se˜nal de reloj SCK que sirve de sincronismo durante la comunicaci´on y una se˜nal de dato SDIO mediante la cual se transmiten los datos en serie. De especial inter´es son los registros Delta_X (0x03) yDelta_Y (0x04) que contienen valores relacionados con el movimiento en el eje X y el eje Y. Estos datos son el resultado de un algoritmo interno implementado en el DSP para el c´alculo de Flujo ´ Optico y no ser´an usados ya que el inter´es de este trabajo reside en implementar el algoritmo de interpolaci´on presentado anteriormente. Por otro lado, la lectura del frame se realiza mediante la lectura sucesiva del registro PixelData (0x08) hasta recorrer todos los pixeles del frame. Todas las transacciones de lectura y escritura con el chip son de 8 bits. El registro PixelData devuelve el valor del pixel medido en sus 6 bits menos significativos PD<5:0>, los dos bits restantes son bits de control, el bit SOF es un bit que identifica el primer pixel de un frame y el bit DATA_VALID permite comprobar si el registro contiene un valor v´alido (Ver Figura 4.2). Durante la lectura del frame han de comprobarse ambos bits para leer el frame correctamente. La tasa de transferencia de datos del sensor depender´a de la frecuencia del cristal conectado al chip. Como m´aximo es posible la conexi´on de un cristal de 24MHz y la frecuencia m´axima del reloj en el bus de datos viene dada por: fSCK =fCLK 12 (4.1) por tanto, la m´axima frecuencia permitida para fSCK es 2MHz. Adem´as, ser´a necesario respetar los tiempos entre operaciones de escritura/lectura especificados en la hoja de datos del chip. En la Subsecci´on 4.3.3 se realiza un estudio detallado de los distintos tiempos involucrados en la transferencia del frame del sensor y se calcula la tasa de transferencia en FPS. El consumo del chip oscila entre 12mA y 30mA dependiendo de si el sensor se encuentra en movimiento o permanece quieto [70]. Se indica que el consumo t´ıpico cuando el sensor se est´a moviendo es de 15mA, y puesto que en la operaci´on del sistema el sensor estar´a generalmente en movimiento, este ser´a el valor tomado como referencia para el c´alculo del consumo del sistema. 4.2. Descripci´on de la arquitectura hardware del dispositivo En la Figura 4.3 se muestran los distintos elementos que forman la arquitectura del sistema. Cada uno de los m´odulos ‘EyeOF’ (Ver Subsecci´on 4.2.1) se encuentran relacionados mediante una interfaz SPI con el elemento central de la arquitectura: una placa de desarrollo de la serie NUCLEO del fabricante ST Microelectronics, concretamente la placa STM32L476RG-NUCLEO. Esta placa de desarrollo contiene el microcontrolador STM32L476RG compuesto por un n´ucleo ARM ® Cortex ® -M4 a 80MHz como m´aximo y dise˜nado para aplicaciones de bajo consumo [71]. CAP´ ITULO 4. DISE˜ NO HARDWARE Y FIRMWARE PARA LA ADQUISICI´ ON DEL FRAME Y EL C´ ALCULO DEL FLUJO ´ OPTICO 21 STM32L476RG - NUCLEO Módulo EyeOF Módulo EyeOF Periférico SPI1 Periférico SPI2 UART PWM/RC Aplicación Eyes OF Gimbal Platform STorm32 BGC Controlador Gimbal Figura 4.3: Arquitectura del dispositivo Este microcontrolador posee una gran cantidad de perif´ericos hardware, lo que resulta muy ´util en fases de prototipo donde distintas funcionalidades pueden surgir durante el proceso de dise˜no. Entre las caracter´ısticas que han desembocado en la elecci´on de este chip como n´ucleo del sistema se encuentran las siguientes: M´ultiples m´odulos SPI: Aunque el est´andar SPI permite que el bus sea compartido, en esta aplicaci´on en espec´ıfico donde se ha hecho una adaptaci´on de las se˜nales del est´andar (Ver Subsecci´on 4.2.1). Para la comunicaci´on con el sensor ADNS2610 se hace necesario un m´odulo SPI por cada sensor. Ser´ıa posible utilizar un s´olo m´odulo SPI mediante el uso de multiplexores que dirigieran el camino de la se˜nal a cada sensor seg´un sea preciso, sin embargo, en esta ocasi´on se ha decidido usar un m´odulo para cada sensor y disminuir el n´umero de componentes del sistema. DMA (Direct Memory Access): Permite la transferencia de datos entre memoria y perif´ericos o entre distintas zonas de memoria sin la necesidad de hacer uso de la CPU. De esta forma es posible implementar la concurrencia entre tareas de procesamiento y transferencia de datos. Consumo de potencia reducido: Seg´un la documentaci´on del chip, el consumo es de 100 µA/MHz cuando trabaja con un regulador LDO como fuente de alimentaci´on, como es el caso de la tarjeta NUCLEO elegida, por tanto, se tiene un consumo aproximado de 8 mA si se trabaja a la m´axima velocidad permitida (80 MHz). Teniendo en cuenta el consumo medio del sensor ADNS2610 (Ver Secci´on 4.1) se tiene un consumo m´aximo total de: 15 mA ∗2+8mA =38 mA@5 V →P=190 mW 22 4.2. DESCRIPCI´ ON DE LA ARQUITECTURA HARDWARE DEL DISPOSITIVO Ha de anotarse que este consumo se relaciona con la adquisici´on del frame y el c´alculo de Flujo ´ Optico y puede servir de referencia en dise˜nos donde se desee implementar este sistema. En secciones posteriores se har´a referencia al consumo total para la aplicaci´on de seguimiento desarrollada en este trabajo. 4.2.1. M´odulo EyeOF La integraci´on del sensor ADNS2610 en el sistema se ha realizado mediante el dise˜no de un peque˜no m´odulo denominado ‘EyeOF’. El m´odulo consta de tres elementos fundamentales: PCB con electr´onica de adaptaci´on: Esta PCB ha sido dise˜nada y fabricada ad hoc para la implementaci´on del sistema. Se trata de una PCB que incluye todos los componentes necesarios para el correcto funcionamiento del chip, as´ı como un conector con las se˜nales necesarias para su alimentaci´on y comunicaci´on. Soporte para lente montura C: Permite la sujeci´on de una lente de montura C en la posici´on correcta, es decir, a una distancia entre el plano del sensor y el plano de la lente igual a la distancia focal y alineada con el centro de la zona activa del sensor ADNS2610. Lente de montura C: Cualquier lente de montura C es compatible con el m´odulo. En este caso se ha utilizado una lente C 1/2 4-12mm manual Iris de TAMRON. 2 3 4 1 5 6 Figura 4.4: M´odulo EyeOF: Renderizado 3D Dise˜no de la PCB con electr´onica de adaptaci´on El dise˜no de la PCB se ha desarrollado en el software Eagle ® yFusion 360 ® de la compa˜n´ıa AutoDesk. Este software permite desde la confecci´on del esquema el´ectrico que define las conexiones de la PCB (Figura 4.5) hasta el renderizado 3D de la PCB una vez finalizado el posicionamiento de los componentes y las pistas (Figura 4.4). Como es posible comprobar en la Figura 4.5 la PCB contiene adem´as del sensor ADNS2610 1 , un cristal de cuarzo de 24MHz junto con sus capacidades de carga 2 , un conector de cuatro pines para alimentaci´on y se˜nales de comunicaci´on 3 , condensadores de desacoplo 4 y una matriz de pads por si se necesitara de alg´un componente m´as no contemplado en el m´odulo 5 . Adem´as se ha a˜nadido el footprint de un oscilador en formato Through-Hole 6 para tener disponible un abanico m´as amplio de osciladores posibles donde elegir. S´olo se soldar´a a la placa los componentes 2 o 6 , es decir, estos dos componentes no deben coexistir en la PCB. CAP´ ITULO 4. DISE˜ NO HARDWARE Y FIRMWARE PARA LA ADQUISICI´ ON DEL FRAME Y EL C´ ALCULO DEL FLUJO ´ OPTICO 23 R afael de la R osa Vidal Máster en Microelectrónica Universidad de S evilla 2.2u 2.2u GND +5V 47k GND +5V GND 1 4 S 4B-PH-S M4-TB NX3225S A 10p 10p GND GND 6X3_PAD_MATR IX OS C (IN) OS C (OUT ) S DIO S C KLE D-C T L VDD R E FA GND U1 ADNS -2610 C1 C2 R 1 S 1 X1 C3 C4 TP 1 X2 C5 C6 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 J1 S heet: A B C D 1 2 3 4 5 6 A B C D 1 2 3 4 5 6 eyeOF_module 10/09/2020 19:04 1/1 Figura 4.5: Esquema el´ectrico del m´odulo EyeOF 24 4.2. DESCRIPCI´ ON DE LA ARQUITECTURA HARDWARE DEL DISPOSITIVO Figura 4.6: Dise˜no 3D de cada uno de los componentes del m´odulo y su ensamblaje Dise˜no mec´anico del soporte y la lente Los elementos que forman el dispositivo deben posicionarse de forma que la luz procedente del entorno sea dirigida al plano activo del sensor ADNS2610 a trav´es de la lente del m´odulo. Para conseguir un resultado ´optimo en la construcci´on del dispositivo, se decide realizar un dise˜no 3D del conjunto del dispositivo en escala 1:1 en el software Fusion 360 ® y, de esta forma, medir las distintas dimensiones de inter´es en el montaje. En la Figura 4.6 aparecen cada uno de los modelos 3D dise˜nados, as´ı como el ensamblaje final de todas las piezas. En [72] es posible consultar las distintas dimensiones relevantes para el dise˜no. Una montura de lente tipo C consta de una rosca 1-32 TPI y la distancia entre el panel frontal de la lente y la superficie del sensor de imagen debe ser de 0.69 inch, o lo que es lo mismo, 17.526 mm. Fusion 360 ® permite obtener la secci´on de un conjunto 3D y medir sobre el plano resultante. En la Figura 4.7 se muestra la medida de la distancia focal una vez dimensionado el soporte para que esta se cumpla. Es necesario anotar que debido a que las dimensiones internas del chip no son conocidas, se ha estimado que el dado del chip se encuentra aproximadamente a la misma altura que los pines del encapsulado. Adem´as, la medida de la distancia focal en la Figura 4.7 se ha realizado en condiciones ideales y habr´a que tener en cuenta las tolerancias de la fabricaci´on 3D para corregir posibles desajustes en el dise˜no. Una vez fabricada la pieza, la lente elegida permite un ajuste fino de la distancia focal, por lo que la tolerancia permitida es relativamente alta. 4.2.2. Construcci´on del dispositivo Finalizado el dise˜no se procede a la fase de construcci´on del dispositivo. Para ello es necesario reunir cada uno de los componentes listados anteriormente en la Figura 4.6. La PCB se envi´o a fabricar externamente, el adaptador de rosca 1-32 TPI es est´andar y junto con la lente se obtuvo de un proveedor y la estructura de uni´on de la lente y la PCB fue fabricada en una de las impresoras 3D disponibles en el IMSE. En la Figura 4.8 se muestra el resultado obtenido tras la construcci´on de los dos m´odulos EyeOF junto con los cables necesarios para conectarlos a la placa STM32L476RG a trav´es de un HAT de prototipado. Uno de los m´odulos se ha mantenido sin lente para que se pueda observar el interior del m´odulo en la figura. CAP´ ITULO 4. DISE˜ NO HARDWARE Y FIRMWARE PARA LA ADQUISICI´ ON DEL FRAME Y EL C´ ALCULO DEL FLUJO ´ OPTICO 25 17.526mm Figura 4.7: Secci´on del m´odulo EyeOF y medida de la distancia focal necesaria para cumplir con el est´andar de montura tipo C Figura 4.8: M´odulos EyeOF conectados a la placa STM32L476RG Una vez la construcci´on sea definitiva y se asegure que no se va a desmontar el m´odulo nuevamente se retirar´a la pared superior del encapsulado para permitir que le llegue la m´axima cantidad 32 4.3. DESCRIPCI´ ON DE LA ARQUITECTURA FIRMWARE DEL DISPOSITIVO 17 35 53 71 89 107125143161179197215233251269287305323 16 34 52 70 88 106124142160178196214232250268286304322 15 33 51 69 87 105123141159177195213231249267285303321 14 32 50 68 86 104122140158176194212230248266284302320 13 31 49 67 85 103121139157175193211229247265283301319 12 30 48 66 84 102120138156174192210228246264282300318 11 29 47 65 83 101119137155173191209227245263281299317 10 28 46 64 82 100118136154172190208226244262280298316 9 27 45 63 81 99 117135153171189207225243261279297315 8 26 44 62 80 98 116134152170188206224242260278296314 7 25 43 61 79 97 115133151169187205223241259277295313 6 24 42 60 78 96 114132150168186204222240258276294312 5 23 41 59 77 95 113131149167185203221239257275293311 4 22 40 58 76 94 112130148166184202220238256274292310 3 21 39 57 75 93 111129147165183201219237255273291309 2 20 38 56 74 92 110128146164182200218236254272290308 1 19 37 55 73 91 109127145163181199217235253271289307 0 18 36 54 72 90 108126144162180198216234252270288306 Figura 4.12: Tabla de ´ındices: En azul los ´ındices en los que alg´un coeficiente de la operaci´on de Flujo ´ Optico puede ser calculado; en rojo los ´ındices en los que no. 13 31 49 67 85 12 30 48 66 84 11 29 47 65 83 10 28 46 64 82 9 27 45 63 81 8 26 44 62 80 7 25 43 61 79 6 24 42 60 78 5 23 41 59 77 4 22 40 58 76 3 21 39 57 75 2 20 38 56 74 1 19 37 55 73 0 18 36 54 72 Figura 4.13: Indices tomados en el c´alculo de cada coeficiente. Avance del c´alculo de coeficientes. CAP´ ITULO 4. DISE˜ NO HARDWARE Y FIRMWARE PARA LA ADQUISICI´ ON DEL FRAME Y EL C´ ALCULO DEL FLUJO ´ OPTICO 33 Un momento respecto al eje perpendicular al plano donde se encuentran dispuestos los m´odulos y que pasa por el centro geom´etrico de los dos m´odulos resultado de la resta entre las componentes en el eje ydel frame. Esto se traduce en informaci´on sobre la rotaci´on del escenario frente a los m´odulos. Esta operaci´on se realiza en la funci´on OF_ComputeFused disponible en el Anexo C. 4.3.6. Transferencia de datos al PC Todos los datos que genera el dispositivo se almacenan en estructuras empaquetadas en memoria. De esta forma, es posible realizar la transferencia de estos datos a trav´es de una USART mediante DMA. La estructura y la descripci´on de sus miembros se encuentra en la Tabla 4.2 y es posible consultarla en el c´odigo en el Anexo D bajo el nombre frameStruct. Solo es necesario indicar al perif´erico DMA la direcci´on donde comienza el bloque de memoria que se desea transferir y su longitud. El bloque hardware realizar´a la transferencia y avisar´a a la CPU mediante una interrupci´on cuando se haya completado. De esta forma, se puede verificar que no se corrompen los datos al empezar la transferencia mientras la anterior sigue en curso. Mientras el perif´erico DMA realizar la transferencia de datos, la CPU contin´ua procesando el frame actual. S´olo se necesitan un n´umero reducido de instrucciones para configurar el bloque hardware. De otra forma, la CPU estar´ıa ocupada un gran n´umero de instrucciones durante la transferencia y la capacidad de procesamiento de los frames disminuir´ıa en gran proporci´on. Common Plane Figura 4.14: Disposici´on espacial de los m´odulos EyeOF en el sistema. En rojo los sistemas de coordenadas relacionados con cada uno de los m´odulos EyeOF del sistema; en azul el sistema de coordenadas para el vector resultado de la fusi´on y el eje sobre el cual se aplica el momento resultado de la fusi´on. 34 4.3. DESCRIPCI´ ON DE LA ARQUITECTURA FIRMWARE DEL DISPOSITIVO Dato Tipo de estructura Descripci´on Header uint32_t Encabezado conocido del paquete de datos N´umero de secuencia uint8_t N´umero de secuencia del paquete, es posible detectar si ha existido alg´un error en la transferencia si dos paquetes no contienen n´umeros de secuencia consecutivos Im´agenes uint8_t[2][324] Contienen los valores de los pixeles obtenidos en la lectura de las im´agenes de los dos sensores ADNS2610 del dispositivo Flujo ´optico int32_t[7] Contiene los valores de los diferentes valores del Flujo ´ Optico (para las im´agenes de cada sensor y el resultado de la fusi´on de ambos sensores) Estado del sensor uint8_t Reporte del estado del dispositivo en distintos subconjuntos de bits en el registro Tabla 4.2: Estructura de datos del bloque de memoria transferido al PC y su descripci´on Cap´ıtulo 5 Dise˜no y control de la plataforma m´ovil El array de m´odulos EyeOF desarrollado en el Cap´ıtulo 4 debe tener libertad de movimiento en el espacio tridimensional. Es necesario de un soporte mec´anico y un controlador que a˜nada esta capacidad al sistema: a partir de la informaci´on generada por el Flujo ´ Optico, mover los m´odulos de manera coherente. El dise˜no mec´anico de la plataforma se va a basar en estructuras estabilizadoras denominadas gimbal o card´an, existentes en el mercado y que son utilizadas para la realizaci´on de videos estabilizados (sin vibraci´on). Este tipo de sistemas suelen integrarse en UAVs con c´amaras a bordo, ya que de otro modo, la din´amica del UAV y las perturbaciones como las corrientes de viento har´ıan imposible la captura im´agenes o v´ıdeos de forma ´optima. Como ejemplo de este tipo de sistemas, se tiene el dron DJI Phantom 4 Pro v2.0 [75] de la marca DJI mostrado en la Figura 5.1. Por otro lado, es necesario un sistema de control que genere las se˜nales de control necesarias para mantener los motores contenidos en la plataforma en la posici´on deseada. Generalmente los motores utilizados en los gimbals son motores DC brushless de baja velocidad. Como ejemplos de sistemas de control para gimbal con motores DC brushless se tiene STEVAL-GMBL02V1 de ST Microelectronics [76] , AlexMos de BaseCam [77] o STorM32-BGC [78] que es un proyecto desarrollado y soportado por una gran comunidad OpenSource y actualmente es ampliamente utilizado en el mundo de los UAVs con c´amaras estabilizadas integradas. Este ´ultimo es el sistema que se va a utilizar en este trabajo. En las siguientes secciones se va a desarrollar cada elemento que compone la plataforma m´ovil y las decisiones de dise˜no tomadas durante su desarrollo. Aspectos como el modelado mec´anico de la plataforma y los motores no se realizan debido a que se encuentra fuera del alcance del trabajo. Se har´a m´as hincapi´e en la parte de control y en la adaptaci´on del controlador necesaria para su uso con los m´odulos EyeOF. En cualquier caso siempre se dar´an las referencias adecuadas que permitir´an al lector profundizar en temas de su inter´es y que en este cap´ıtulo no se traten. 5.1. Motores DC brushless La construcci´on de un motor DC brushless es muy parecida a la construcci´on de los motores DC brushed. En estos ´ultimos se tiene un est´ator con imanes permanentes y un rotor con bobinas que hacen de electroimanes. Al alimentar el motor con una tensi´on DC VM, por acci´on de las escobillas se genera una tensi´on AC sobre las bobinas del rotor, que a su vez, genera un campo magn´etico 35 36 5.1. MOTORES DC BRUSHLESS Cámara con gimbal o cardán Figura 5.1: Dron DJI Phatom 4 Pro v2.0 de la marca DJI que contiene un gimbal o card´an para estabilizar la c´amara que interacciona con los imanes permanentes del est´ator y produce la rotaci´on. Un esquema b´asico de la construcci´on de un motor DC brushed es mostrado es la Figura 5.2 (a). En los motores DC brushless (Ver Figura 5.2 (b)), los imanes permanentes se disponen en el rotor y las bobinas en el est´ator. Las escobillas son reemplazadas por un inversor electr´onico externo al motor, es decir, los motores DC brushless deben ser alimentados con tensiones AC [79, 80, 81]. Como consecuencia el control de estos motores es m´as complejo debido a la necesidad de la generaci´on de se˜nales AC aplicadas a los terminales de entrada (VA,VByVCen la Figura 5.2 (b)) siguiendo un patr´on adecuado seg´un la posici´on del rotor. En Wach [81] se desarrollan distintos m´etodos para controlar este tipo de motores. Los motores DC brushless presentan una serie de ventajas relevantes en esta aplicaci´on respecto a otros motores: Su peso es reducido, por tanto la plataforma es m´as ligera y necesita de menos potencia para su movimiento. Este tipo de motores permite desarrollar el m´aximo torque durante toda la rotaci´on, siempre y cuando el control sea el ´optimo. Otros motores solo alcanzan el torque m´aximo en ciertos puntos de la rotaci´on. Permiten el control de su velocidad y su aceleraci´on en el movimiento entre posiciones, lo que se traduce en movimiento m´as suaves que mejora la calidad de la estabilizaci´on. Otros motores como los motores paso a paso o los servomotores permiten el control de su posici´on pero no de la curva de velocidad y aceleraci´on entre posiciones, produci´endose movimientos bruscos que degradan el rendimiento de la estabilizaci´on. En este trabajo se van a utilizar motores brushless 2208 de 90 KV y 14 polos1. El valor KV de un motor brushless es una especificaci´on que aporta el fabricante que permite conocer a priori si el motor es adecuado para la aplicaci´on deseada. Este valor depende fundamentalmente del n´umero de espiras en las bobinas del motor, el di´ametro del hilo de cobre utilizado en dichas bobinas, la potencia de los imanes permanentes y la geometr´ıa del motor; y se refiere al n´umero de revoluciones por minuto (RPM) que es capaz de ofrecer el motor cuando se le aplique se˜nales de 1 voltio de 1n´umero de imanes permanentes en el rotor y bobinas en el est´ator CAP´ ITULO 5. DISE˜ NO Y CONTROL DE LA PLATAFORMA M´ OVIL 37 VM VA VB VC N N S S S SN N Estátor Rotor Escobillas (a) (b) Figura 5.2: Construcci´on b´asica de motores DC brushed (a) y motores DC brushless (b) amplitud sin existencia de carga sobre el eje del motor. Para aplicaciones de gimbal se suelen usar motores DC brushless en un rango de 50 KV a 150 KV, mientras que para otras aplicaciones como motores para h´elices de drones se utilizan motores con valores KV mucho m´as altos, de 1200 KV en adelante. Por ´ultimo, ha de tenerse en cuenta la resistencia que presenta el motor en cada fase, ya que esto determina la cantidad de corriente que puede llegar a demandar el motor. En este caso, seg´un los drivers del controlador que se va a usar, se necesita de un motor de m´as de 10 Ω por fase. En el caso del motor elegido se tienen 13 Ω. 5.2. Controlador STorM32 BGC El controlador STorM32 BGC es un proyecto que nace en una gran comunidad OpenSource dedicada al mundo de los drones. En esta comunidad se desarrollan ideas procedentes de necesidades de los miembros, as´ı como se plantean problemas que abordan en conjunto, aportando cada miembro el conocimiento que tenga sobre la problem´atica. El controlador STorM32 BGC [78] permite el control de un sistema gimbal o card´an de tres ejes implementado mediante motores brushless. En esta secci´on se va a explicar las principales caracter´ısticas del controlador, c´omo configurarlo y qu´e funciones se van a utilizar en esta aplicaci´on. En la Figura 5.3 se muestra la planta de la tarjeta junto con indicaciones sobre algunas de sus conexiones externas. Las conexiones superiores corresponden a los motores para los ejes de Pitch, Roll y Yaw (Ver Secci´on 5.3 para m´as informaci´on sobre estos t´erminos). Cada pin se corresponde a una fase (VA,VByVC) como las mostradas en la Figura 5.2 (b), si se tienen m´as de 3 bobinas en el motor, se interconectan de manera coherente para activar grupos de bobinas al actuar sobre cada una de las fases. En la zona inferior se muestra la conexi´on a la alimentaci´on principal de la tarjeta2. Los valores de tensi´on admitidos se encuentran en el rango de 9V a 18V, con un consumo de potencia m´aximo en torno a los 60W. En este caso particular se ha optado por una fuente de alimentaci´on DC de 12V y 5A (60W). A la izquierda se tienen las conexiones a 3 elementos: 2La tarjeta tambi´en puede ser alimentada mediante la conexi´on USB, aunque se utilizar´a esta siempre que est´e disponible. 38 5.2. CONTROLADOR STORM32 BGC Pitch Motor Roll Motor Yaw motor Radio Control Signals External IMU DC power USB to PC Motor drivers On-board IMU Microcontroller Figura 5.3: Planta de la tarjeta STorM32 BGC conexiones a se˜nales de Radio Control que permiten modificar la posici´on del sistema; una IMU3 que permite conocer la posici´on de los m´odulos, se colocar´a en el mismo plano que los m´odulos y ser´a conectada al controlador mediante un cable trenzado (Ver Secci´on 5.3); conexi´on USB para la configuraci´on de la tarjeta desde el PC. El n´ucleo de la tarjeta es el microcontrolador STM32F103RCT6 de ST Microelectronics con arquitectura ARM ® 32-bit Cortex ® -M3 a 72 MHz [82]. Este dispositivo se encarga de realizar todos los c´alculos relacionados con el control del sistema, la generaci´on de las se˜nales de control y la recepci´on de se˜nales externas, como las se˜nales de Radio Control. Como interfaz entre los motores y las se˜nales de control generadas por el microcontrolador se tienen los drivers DRV8313 [83]. Constituidos por 3 etapas push-pull N-MOSFET son adecuados para el control de cargas inductivas como solenoides o motores brushless. Permiten ser alimentados por 65 V como m´aximo, tienen capacidad de corriente de hasta 2,5 A e incluyen protecciones de sobretensi´on, sobrecorriente y sobretemperatura que son notificadas mediante uno de sus pines. El controlador basa su funci´on de control en el conocimiento de la posici´on de la tarjeta y la posici´on del objeto que se desea posicionar (en este caso los m´odulos EyeOF) [84, 85], para ello dispone de dos IMU3MPU6050 [86, 87]: una integrada en la tarjeta y otra externa dispuesta junto el objeto a posicionar. Este circuito integrado se conecta mediante una interfaz I2C al microcontrolador y permite leer medidas de los gir´oscopos a 8 kHz y medidas de los aceler´ometros a 1 kHz. En Kok, Hol, and Sch¨on [88] se describen diversos algoritmos para la estimaci´on de la posici´on y la orientaci´on de un cuerpo a partir de la informaci´on entregada por IMUs. Este tema no se aborda en este trabajo puesto que es un campo muy amplio y se escapa de su alcance. El controlador STorM32 BGC permite un grado de configurabilidad elevado. Entre las opciones que ofrece se encuentra la compatibilidad con diversas configuraciones de gimbal, la compatibilidad 3“Inertial Measurement Unit (IMU)” es un dispositivo constituido fundamentalmente por un gir´oscopo de 3 ejes y un aceler´ometro de 3 ejes construidos generalmente en tecnolog´ıas MEMS. CAP´ ITULO 5. DISE˜ NO Y CONTROL DE LA PLATAFORMA M´ OVIL 39 con diferentes est´andares de Radio Control (PWM [89], Spektrum Satellite [90], Futaba S-BUS [91], HoTT SUMD [92], entre otros) y el ajuste de los par´ametros PID del controlador para poder estabilizar diferentes din´amicas. Todas estas opciones se encuentran disponibles a trav´es de la GUI que aparece en la Figura 5.4. En las siguientes secciones se ir´an comentando los apartados de la aplicaci´on relevantes en esta implementaci´on. Figura 5.4: GUI para configuraci´on del controlador STorM32 BGC 5.3. Dise˜no mec´anico de la plataforma Para el dise˜no mec´anico de la plataforma se ha utilizado el software de modelado 3D Fusion 360 ® de la compa˜n´ıa AutoDesk, que, como se coment´o en la Secci´on 4.2.1 permite integrar el dise˜no electr´onico de los m´odulos EyeOF desde el software de dise˜no el´ectrico Eagle ® . El modelo 3D de la plataforma va a permitir estudiar la disposici´on de todos los elementos constituyentes (motores, estructuras de soporte, dispositivos de control, etc.), estudiar la posici´on relativa de cada elemento para evitar colisiones en el movimiento y generar archivos binarios (STL [93]) compatibles con m´etodos de fabricaci´on aditiva (impresi´on 3D [93], en este caso) que permitir´an obtener las piezas necesarias para la posterior construcci´on de la plataforma. Para la representaci´on de la posici´on y la actitud de un cuerpo en un espacio 3D se hace necesario de la definici´on de un sistema de referencia inercial (“Inertial frame4” en la Figura 5.5, que es fijo y no sufre ning´un tipo de movimiento), y un sistema de referencia local al cuerpo cuyo origen de coordenadas es su centro de gravedad (“Body frame4” en la Figura 5.5), sus ejes se disponen como se muestra en la Figura 5.5 y es no inercial. Sobre el sistema de referencia local al cuerpo existen dos formas de definir la actitud de un objeto: mediante los ´angulos de Euler o 4No confundir la traducci´on al ingl´es de sistema de referencia a frame y el concepto de frame definido en el Cap´ıtulo 3. 40 5.3. DISE˜ NO MEC´ ANICO DE LA PLATAFORMA MPU 6050 module STorM32 BGC controller Pitch Roll Yaw The slots allow balancing the gimbal z yx Inertial frame Body frame Figura 5.5: Dise˜no mec´anico de la plataforma: modelo 3D del soporte de los m´odulos mediante cuaterniones [85]. Los ´angulos de Euler se definen como sigue: Pitch: ´ Angulo de rotaci´on respecto al eje X del sistema de referencia inercial. Roll: ´ Angulo de rotaci´on respecto al eje Y del sistema de referencia inercial. Yaw: ´ Angulo de rotaci´on respecto al eje Z del sistema de referencia inercial. Este tipo de representaci´on de la actitud es ambigua cuando dos de los ejes se posicionan en paralelo (Ver “Gimbal Lock Problem” en [94]). Para solucionar esta singularidad se tiene la representaci´on mediante cuaterniones. Un cuaterni´on es un vector cuadrimensional en la que la primera de sus componentes representa un ´angulo y las tres componentes restantes describen el eje sobre el que se gira el ´angulo definido por la primera componente. Cada posici´on en el espacio 3D da lugar a un cuaterni´on valor ´unico. En la plataforma que se desarrolla en este trabajo, los sistemas de referencia inercial y local al cuerpo vienen definidos por la posici´on de las IMUs. En la Secci´on 5.2 se coment´o que el controlador CAP´ ITULO 5. DISE˜ NO Y CONTROL DE LA PLATAFORMA M´ OVIL 41 Figura 5.6: Modelo 3D del sistema y vistas ortogonales con movimientos a ±45◦ 48 5.4. DEFINICI´ ON E IMPLEMENTACI´ ON DE LA FUNCI´ ON DE CONTROL Figura 5.13: Configuraci´on de las se˜nales de Radio Control rresponde con uno de los pines del conjunto correspondiente a las se˜nales de Radio Control (Ver Figura 5.3): RC-0 para el Pitch, RC-1 para el Roll y RC-2 para el Yaw. Para cada se˜nal se puede observar que existen una serie de par´ametros configurables como son el modo, el rango para los ´angulos permitidos en cada eje y los limites en la aceleraci´on y la velocidad adoptada en el movimiento de la plataforma. Para este caso particular se tiene: El modo de todas las se˜nales ser´a el modo absoluto. En este modo la plataforma volver´a a la posici´on inicial configurada si deja de recibir se˜nales de Radio Control. Los l´ımites para los ´angulos de rotaci´on desde la posici´on neutral en cada eje se han ajustado de forma que el cableado de la plataforma no interfiera en la operaci´on del sistema. Los l´ımites de velocidad y aceleraci´on se han disminuido lo suficiente para no provocar sobreoscilaciones en el movimiento. Se desea que el movimiento sea lo m´as r´apido posible. Que estos l´ımites puedan ser de menor valor depender´a de los ajustes realizados en los reguladores PID y de las caracter´ısticas din´amicas de la plataforma. Por ´ultimo, relacionado con la configuraci´on de las se˜nales de control se tienen los par´ametros RC Dead Band yRC Hysteresis. Ambos par´ametros se encuentran relacionados con la presencia de jitter en la se˜nal de Radio Control [78]. Este ruido depende de la fuente que genera dicha se˜nal y puesto que en este caso es generada por un microcontrolador con una resoluci´on temporal en el orden de los nanosegundos su valor se ha igualado a cero. Todos los dem´as par´ametros contenidos en este apartado de la interfaz no son utilizados y por ello se dejan con su valor por defecto. CAP´ ITULO 5. DISE˜ NO Y CONTROL DE LA PLATAFORMA M´ OVIL 49 Para conocer el tiempo que debe esperar el sistema desde que se env´ıa una se˜nal de control hasta que la plataforma realiza el cambio de posici´on indicado se estudia la respuesta al escal´on unitario de la plataforma. Para ello, la interfaz de configuraci´on permite grabar los valores de las se˜nales de control entrantes as´ı como la salida de los gir´oscopos de cada eje de la IMU contenida en el m´odulo MPU 6050 con una resoluci´on de decenas de milisegundos. En la Figura 5.14 se muestran una gr´aficas realizadas a partir de los datos obtenidos de la plataforma con el software Matlab ® . Uno de los par´ametros m´as utilizados para la caracterizaci´on de respuestas din´amicas en sistemas de primer orden [95, 96] es la constante de tiempo (τ), que viene dada por el tiempo transcurrido desde que se aplica el est´ımulo hasta que la se˜nal toma un valor igual al 63 % (1 − exp(−t /τ)=0,63) de su valor final. Los valores de las constantes de tiempo para cada eje vienen anotados en las gr´aficas correspondientes. Para un sistema de primer orden se tiene que el tiempo de establecimiento es aproximadamente igual a tres veces la constante de tiempo. En este caso, los tiempos de establecimiento van desde 366 ms para el Pitch hasta 462 ms para el Yaw. El tiempo de retardo que debe adoptar el sistema para esperar a que la plataforma se posicione se encuentra en este rango de tiempos, lo que limita la operaci´on de la plataforma al seguimiento de objetos que se muevan a una velocidad moderada y, que adem´as, se encuentren alejadas de esta, ya que cuanto m´as alejado est´e el objetivo, mayor ser´a la distancia equivalente en el espacio para un movimiento de la plataforma de la misma magnitud. El objetivo es disminuirlo al m´aximo y se deber´a ajustar junto con el regulador PID que se describe a continuaci´on para conseguir la respuesta deseada. 0 200 400 600 800 1000 1200 1400 Time (ms) 0 20 40 60 80 100 Hundreds of degrees Pitch 0 200 400 600 800 1000 1200 1400 Time (ms) 0 20 40 60 80 100 Hundreds of degrees Roll 0 200 400 600 800 1000 1200 1400 Time (ms) 0 20 40 60 80 100 Hundreds of degrees Yaw X 331 Y 0 X 453 Y 63 X 316 Y 0 X 443 Y 63 X 255 Y 0 X 409 Y 63 Figura 5.14: Respuestas din´amica de la actitud del sistema a una entrada de tipo escal´on unitario En la funci´on applyControlLaw en el Anexo E se muestra el c´odigo que implementa los tres reguladores PID, uno para cada eje de la plataforma. La entrada a los reguladores es el valor de 50 5.4. DEFINICI´ ON E IMPLEMENTACI´ ON DE LA FUNCI´ ON DE CONTROL Flujo ´ Optico calculado, que representa el error entre las im´agenes anteriores y las im´agenes actuales capturadas por los m´odulos EyeOF. Se conoce de [97] que la respuesta del sensor de Flujo ´ Optico desarrollado es lineal en un rango de error entre los frames. Los valores de KP,KIyKDpara cada regulador implementado vienen dados por los valores de los macros definidos en la zona superior del Anexo E: PITCH_P,PITCH_D,PITCH_I,ROLL_P,ROLL_D,ROLL_I,YAW_P,YAW_D yYAW_I. Estos valores han de ajustarse mediante prueba y error, primer se realiza un preajuste para cada uno de los ejes por separado y finalmente se hace un ajuste fino con los tres ejes activados. Adem´as, se disponen dos par´ametros de ajuste adicionales que permiten modificar la acci´on de control del regulador PID. Por un lado, los valores PITCH_WINDUP,ROLL_WINDUP yYAW_WINDUP ajustan el fen´omeno conocido como “Integral Windup” [95] que se da en implementaciones de reguladores PID. Este fen´omeno se produce cuando el actuador que modifica la salida del sistema satura, es decir, se encuentra en uno de los extremos de su rango de operaci´on, lo que produce un r´apido incremento del t´ermino integral del controlador PID que puede terminar desestabilizando al sistema. El valor de ajuste implementado permite poner a cero el valor de la integral cuando su valor alcance el valor indicado en cada par´ametro. Por otro lado, se tiene el valor DELTALIMIT que indica el valor m´ınimo a la salida del regulador PID que genera una acci´on del actuador. As´ı, se˜nales de control de peque˜na magnitud, que pueden dar lugar a vibraciones no deseadas, son evitadas. Para ayudar en el proceso de configuraci´on y ajuste del sistema en el siguiente cap´ıtulo se desarrolla una interfaz de escritorio para monitorizar la informaci´on generada por la plataforma y controlar las distintas funciones que permite. Cap´ıtulo 6 Desarrollo de la interfaz de usuario Las diferentes funcionalidades y medidas que realiza la plataforma se encuentran disponibles a trav´es de una aplicaci´on de escritorio desarrollada bajo la tecnolog´ıa WPF de Microsoft incluida en las versiones posteriores a .NET Framework 3.0 y .NET Core 3.0 para el sistema operativo de Windows. En las secciones que siguen se desarrollaran las diferentes caracter´ısticas que ofrece dicha tecnolog´ıa, as´ı como el proceso de dise˜no gr´afico y funcional de la aplicaci´on. 6.1. Tecnolog´ıa WPF y .NET Core WPF son las siglas de Windows Presentation Fundation y se trata de una API que permite el desarrollo de aplicaciones de escritorio enriquecidas y sofisticadas para el sistema operativo Windows [98]. Entre las principales ventajas de esta herramienta se encuentra la integraci´on con DirectX para el renderizado altamente eficiente de los contenidos gr´aficos y la divisi´on entre el dise˜no gr´afico y el comportamiento de la interfaz que permite la implementaci´on del paradigma Model-View-ViewModel (MVVM) muy ´util en el desarrollo de aplicaciones de escritorio. Por otro lado, en los ´ultimos a˜nos Microsoft ha desarrollado una nueva versi´on de .NET Framework denominada .NET Core cuya principal ventaja es que se encuentra orientado al uso de .NET multiplataforma. Este framework se encuentra en plena fase de evoluci´on y fue hace varios meses cuando incorporaron la tecnolog´ıa WPF al framework. Actualmente, aunque WPF trabaje bajo .NET Core no tiene capacidad multiplataforma, pero se espera que en el futuro s´ı que lo sea y por ello se ha decidido utilizar .NET Core en esta ocasi´on, para poder lanzar esta aplicaci´on en macOS o Linux en un futuro cercano. 6.2. Paradigma de programaci´on MVVM (Model-View-ViewModel) El paradigma MVVM para aplicaciones WPF fue presentado por el arquitecto de software en Microsoft, Gossman [99] en 2005. Se trata de un patr´on de programaci´on software que facilita la partici´on entre el desarrollo de la parte gr´afica y la parte funcional en una aplicaci´on de interfaz de usuario [98]. De esta forma, es posible editar cada una de las partes sin influir en la otra, permitiendo el desarrollo por separado de ambas partes. Este paradigma se encuentra integrado en WPF y lo componen tres elementos principales como se muestra en la Figura 6.1: 51 52 6.3. DISE˜ NO GR´ AFICO DE LA APLICACI´ ON Datos y lógica orientada a la aplicación Aspecto visual y lógica orientada a la interfaz Vinculación datos ModelViewModelView Figura 6.1: MVVM - Bloques funcionales y su relaci´on View: En este bloque se implementan todos los gr´aficos de la interfaz y permite la interacci´on con el usuario. La disposici´on de cada elemento de la aplicaci´on se define mediante lenguaje XAML [100]. El entorno de desarrollo dispone de herramientas que permiten visualizar en todo momento el resultado del c´odigo XAML desarrollado. Model: Contiene todos los datos y m´etodos relacionados con el contenido de la aplicaci´on sin relaci´on con la interfaz de usuario. En este bloque se desarrolla en C# [101] y en ´el se definen las estructura de datos (bases de datos, clases, etc.) y los procesos necesarios para el manejo de dichos datos. ViewModel: Este bloque es el nexo entre el View y el Model. Por un lado, le expone a la View las diferentes propiedades p´ublicas que contiene el Model. Por otro lado, recibe los distintos eventos generados por la View e implementa diferentes funciones l´ogicas interactuando con el Model. Es necesario anotar que la vinculaci´on de los datos entre el View y el ViewModel es implementado bajo la arquitectura de WPF mientras que la llamada a los datos y m´etodos del Model desde el ViewModel es realizada por el usuario en funci´on del comportamiento que se desee en la aplicaci´on. 6.3. Dise˜no gr´afico de la aplicaci´on El nombre de la aplicaci´on es ‘Eyes OF Gimbal Platform’ y permite la visualizaci´on de los datos generados por la plataforma, as´ı como algunas funciones de configuraci´on y posicionamiento. Presenta un estilo moderno basado en un toolkit de Google denominado MaterialDesign [102] que ha sido modificado para ser compatible con WPF. En la Figura 6.2 se muestra la vista inicial de la aplicaci´on. Cada una de las distintas posibilidades que permite la aplicaci´on se habilitar´an en funci´on de las funciones habilitadas anteriormente. De esta forma la interacci´on con el usuario es segura y se proh´ıbe acciones no permitidas. En la Secci´on 6.4 se desarrollar´an cada una de las funciones y el procedimiento a seguir para su implementaci´on. 6.3.1. Monitorizaci´on del frame En la interfaz se muestran los frames adquiridos por ambos sensores en tiempo real. Para ello se utiliza la clase interna de C# WriteableBitmap [103] que permite crear y modificar frames a CAP´ ITULO 6. DESARROLLO DE LA INTERFAZ DE USUARIO 53 Figura 6.2: Eyes OF Gimbal Platform - vista inicial nivel de pixel. Esta clase contiene dos buffers, un buffer en segundo plano que contiene informaci´on que no se est´a mostrando y un buffer frontal que contiene la informaci´on que se est´a mostrando actualmente. Un sistema de renderizado interno a la clase copia el contenido del buffer secundario al buffer frontal para su visualizaci´on. Cada uno de los buffers son gestionados mediante dos hilos de procesamiento distintos (Ver figura 6.3). Por un lado, el buffer frontal es gestionado por el hilo de renderizaci´on y por otro lado el buffer en segundo plano es gestionado por el hilo de interfaz gr´afica (‘UI Thread’). El hilo gr´afico escribe datos en el buffer secundario y el hilo de renderizado lee el contenido del buffer frontal y los copia a la memoria de video. El intercambio de datos entre el buffer frontal y el buffer secundario se realiza mediante llamadas a m´etodos que indican qu´e p´ıxeles del frame han cambiado mediante zonas rectangulares del frame. En la Figura 6.4 (a) se muestra un frame adquirido por el m´odulo EyeOF y representado mediante la clase WriteableBitmap. En el frame se muestra un rostro, en el que se puede distinguir perfectamente detalles como los ojos, la boca y la nariz, aunque la resoluci´on del frame sea muy baja. 6.3.2. Monitorizaci´on del Flujo ´ Optico La monitorizaci´on del Flujo ´ Optico generado por el m´odulo EyeOF es muy importante puesto que nos permite verificar el correcto funcionamiento de este. En un principio se limit´o la representaci´on de esta magnitud mediante valores mostrados en la interfaz, sin embargo, este tipo de representaci´on puede resultar confusa y dif´ıcil de interpretar. Por ello se decide representar esta magnitud mediante vectores sobre el frame (ver Figura 6.4 (b)) cuya magnitud es el valor de Flujo 54 6.3. DISE˜ NO GR´ AFICO DE LA APLICACI´ ON Memoria de video Buffer frontal Buffer secundario Datos de usuario Hilo de renderizado Hilo de gráfico Método para indicar qué zona de la imagen ha cambiado Motor de renderizado Escritura datos modificados Figura 6.3: Flujo de datos en la clase WriteableBitmap de C# ´ Optico y adem´as es posible representar su sentido y direcci´on. De esta forma, la interpretaci´on del Flujo ´ Optico es mucho m´as r´apida y sencilla. WPF no contiene de forma nativa controles para representar vectores, sin embargo, s´ı que permite dibujar l´ıneas y figuras geom´etricas. Esto ha sido lo que ha aprovechado Charles Petzold [104] en la creaci´on de una clase que permite incorporar vectores al c´odigo XAML que define la vista de la interfaz. Por otro lado, para suavizar las transiciones entre los distintos valores de Flujo ´ Optico y que el usuario pueda captar correctamente los cambios se implementa un filtro paso bajas sobre el Flujo ´ Optico recibido desde los m´odulos EyeOF. La funci´on que implementa el filtro se denomina FilteringOF y se puede consultar en el Anexo J. Ha de tenerse en cuenta que esto no afecta a la din´amica del Flujo ´ Optico recibido puesto que la frecuencia de muestreo de los vectores en la interfaz es muy superior a la frecuencia de recepci´on de datos desde los m´odulos EyeOF, s´olo se a˜naden valores intermedios para enriquecer la representaci´on del Flujo ´ Optico. (a) (b) Figura 6.4: Frame de un rostro capturado por el m´odulo EyeOF y representado mediante la clase WriteableBitmap: (a) frame representado sin Flujo ´ Optico; (b) frame representado junto con el Flujo ´ Optico CAP´ ITULO 6. DESARROLLO DE LA INTERFAZ DE USUARIO 55 En la Figura 6.4 (b) se pueden observar dos vectores, uno rojo y otro verde. El vector rojo muestra el resultado del c´alculo de Flujo ´ Optico, es decir, cuanto se ha movido el frame actual respecto al frame de referencia siendo el frame actual el origen del vector. De esta forma, en este caso particular, se tiene que el rostro se ha movido a la derecha, que es lo que expresa el vector verde, y que simplemente es un vector con el mismo m´odulo y direcci´on pero en sentido contrario al vector rojo. El vector verde, ser´a el que se utilice para controlar el sistema de seguimiento, ya que indica la correcci´on que se debe realizar en la posici´on de la plataforma para que el frame captado siga siendo el mismo que el de referencia, es decir, que se siga al objetivo en cuesti´on. El c´odigo XAML que define las vistas para la monitorizaci´on del Flujo ´ Optico y la representaci´on de los frames puede consultarse en el Anexo G . 6.4. Dise˜no funcional de la aplicaci´on La aplicaci´on ‘Eyes OF Gimbal Platform’ permite tanto la monitorizaci´on como el control de la plataforma. De esta forma es posible observar la respuesta de los m´odulos EyeOF en todo momento, realizar tareas de calibraci´on y caracterizaci´on y cambiar entre los distintos modos de funcionamiento posibles. Es necesario seguir un proceso de inicializaci´on de la aplicaci´on (Ver Subsecci´on 6.4.1) por parte del usuario para llegar desde la vista inicial Figura 6.2 a la vista mostrada en la Figura 6.5. Se pueden distinguir dos zonas principales: en la zona de la derecha se encuentran todos los comandos disponibles y en la zona de la izquierda se disponen los distintos datos generados por la plataforma. 6.4.1. Inicializaci´on de la aplicaci´on y comandos disponibles Antes de lanzar la aplicaci´on, la plataforma deber´a ser conectada al PC mediante un puerto USB. Al lanzarla, aparece la ventana mostrada en la Figura 6.2, para comenzar a trabajar con esta es necesario realizar los siguientes pasos previos de configuraci´on: 1. Desplegar la lista de puertos de comunicaci´on (COM) disponibles 3 . En ella debe aparecer el puerto COM relacionado con la plataforma, si existe m´as de uno, deber´a comprobarse en Sistema →Administrador de dispositivos qu´e puerto COM es el correcto. 2. Seleccionar el puerto COM y pulsar el bot´on START dispuesto justo encima 3 . 3. En este punto aparecer´an las secciones 1 y 2 sin las medidas de Flujo ´ Optico que aparecen abajo en la Figura 6.5. 4. Para activar las medidas y la representaci´on del Flujo ´ Optico es necesario pulsar el bot´on OPTICAL FLOW 4 . Cuando lo pulsemos el indicar de este bot´on que se encuentra en su esquina superior derecha cambiar´a a ON indicando que esta opci´on se encuentra activa. 5. Al pulsar el bot´on OPTICAL FLOW 4 tambi´en aparecer´a la secci´on de Flujo ´ Optico fusionado 8 que muestra los valores resultado de la fusi´on de los vectores de Flujo ´ Optico representados en 1 y 2 . Tras realizar todos estos pasos los frames capturados por los m´odulos EyeOF se representar´an en 1 y 2 en tiempo real. Adem´as, de la misma forma, los valores de Flujo ´ Optico recibidos ser´an mostrados en 1 , 2 y 8 . 56 6.4. DISE˜ NO FUNCIONAL DE LA APLICACI´ ON 1 2 3 4 5 6 7 8 Figura 6.5: Eyes OF Gimbal Platform - vista que contiene todas las funcionalidades posibles de la aplicaci´on e ´ındices para especificar cada una de las zonas relevantes 6.4.2. Posici´on de la plataforma y funci´on seguimiento La posici´on de la plataforma se puede controlar mediante el joystick de la interfaz 6 . Cada vez que se presione un bot´on se mover´a una cantidad predefinida en la plataforma hacia la direcci´on y sentido mostrado en las flechas del bot´on. El bot´on central devuelve a la plataforma a su posici´on inicial y los dos botones a cada lado del bot´on que mueve la plataforma hacia abajo (↓) permiten rotar la posici´on de los m´odulos. Una vez se tenga posicionada la plataforma tal que los frames adquiridos contengan la informaci´on relacionada con el objetivo que se desea seguir se puede activar la funci´on seguimiento pulsando el bot´on TRACKING 7 . A partir de este momento la plataforma es controlada por el Flujo ´ Optico y no es posible el control mediante el joystick, una vez deshabilitada la funci´on TRACKING, se habilita de nuevo el control mediante joystick. 6.4.3. Funci´on calibraci´on La funci´on calibraci´on se activa mediante el bot´on CALIBRATION 5 . En este modo de funcionamiento el c´alculo de Flujo ´ Optico se calcula de forma est´atica, es decir: El frame que sirve de referencia para el Flujo ´ Optico, el adquirido en el instante t0, se mantiene constante en el tiempo desde que se pulsa el bot´on CALIBRATION 5 . CAP´ ITULO 6. DESARROLLO DE LA INTERFAZ DE USUARIO 57 El frame actual, el adquirido en t, s´ı cambia en el tiempo, es sobrescrito cada vez que se adquiere un nuevo frame. Cuando se pulsa el bot´on CALIBRATION 5 de nuevo se vuelve al funcionamiento de operaci´on normal. Con esto se consigue que el Flujo ´ Optico se mantenga constante mientras los frames adquiridos por los m´odulos sean los mismos, es decir, si el entorno es est´atico, mientras que la plataforma permanezca est´atica el Flujo ´ Optico es constante. Se recuerda que en el funcionamiento normal del dispositivo, como se desarrolla en la Subsecci´on 4.3.2, el frame tomado en t0se actualiza continuamente por el frame adquirido en ty el nuevo frame adquirido es el correspondiente al instante t. Esta funcionalidad permite observar los valores generados por el Flujo ´ Optico en funci´on del movimiento de la plataforma y el frame adquirido por los m´odulos. Es posible as´ı, recolectar datos para diferentes movimientos de la plataforma y representarlos en una curva, obteniendo una recta de calibraci´on [97]. 64 BIBLIOGRAF´ IA [41] Bo Du, Shihan Cai, and Chen Wu. Object Tracking in Satellite Videos Based on a Multiframe Optical Flow Tracker. IEEE Journal of Selected Topics in Applied Earth Observations and Remote Sensing, 12(8):3043–3055, August 2019. ISSN 1939-1404, 2151-1535. doi: 10.1109/ JSTARS.2019.2917703. [42] Hao Su, Yaran Chen, Shiwen Tong, and Dongbin Zhao. Real-time multiple object tracking based on optical flow. In 2019 9th International Conference on Information Science and Technology (ICIST), pages 350–356, Hulunbuir, China, August 2019. IEEE. ISBN 978-172812-106-2. doi: 10.1109/ICIST.2019.8836764. [43] Yuanlu Wu, Minghao Chen, Yan Wo, and Guoqiang Han. Video smoke detection base on dense optical flow and convolutional neural network. Multimedia Tools and Applications, October 2020. ISSN 1573-7721. doi: 10.1007/s11042-020-09870-x. [44] Tobi Delbruck and Manuel Lang. Robotic goalie with 3 ms reaction time at 4 % CPU load using event-based dynamic vision sensor. Frontiers in Neuroscience, 7:223, 2013. ISSN 1662-453X. doi: 10.3389/fnins.2013.00223. [45] T. Delbr¨uck, B. Linares-Barranco, E. Culurciello, and C. Posch. Activity-driven, eventbased vision sensors. In Proceedings of 2010 IEEE International Symposium on Circuits and Systems, pages 2426–2429, 2010. doi: 10.1109/ISCAS.2010.5537149. [46] R. Pericet-Camara, G. Bahi-Vila, J. Lecoeur, and D. Floreano. Miniature artificial compound eyes for optic-flow-based robotic navigation. In 2014 13th Workshop on Information Optics (WIO), pages 1–3, 2014. doi: 10.1109/WIO.2014.6933290. [47] Dario Floreano, Ramon Pericet-Camara, St´ephane Viollet, Franck Ruffier, Andreas Br¨uckner, Robert Leitel, Wolfgang Buss, Mohsine Menouni, Fabien Expert, Rapha¨el Juston, Michal Karol Dobrzynski, Geraud LEplattenier, Fabian Recktenwald, Hanspeter A. Mallot, and Nicolas Franceschini. Miniature curved artificial compound eyes. Proceedings of the National Academy of Sciences, 110(23):9267–9272, 2013. ISSN 0027-8424. doi: 10.1073/pnas.1219068110. [48] Microchip Technology. dsPIC33F Family Data Sheet, 2005. [49] M. V. Srinivasan. An image-interpolation technique for the computation of optic flow and egomotion. Biological Cybernetics, 71(5):401–415, 1994. ISSN 03401200. doi: 10.1007/ BF00198917. [50] Jl L Barron and Na a Thacker. Tutorial: Computing 2D and 3D optical flow. Imaging Science and Biomedical Engineering Division, Medical School, University of Manchester, (2004):1–12, 2005. [51] James Gibson. The Senses Considered as Perceptual Systems. Westport, Conn, June 1983. ISBN 978-0-313-23961-8. [52] James J. Gibson. On the analysis of change in the optic array. Scandinavian Journal of Psychology, 18(1):161–163, 1977. ISSN 1467-9450. doi: 10.1111/j.1467-9450.1977.tb00272.x. [53] Riyanto Sigit and Eva Rochmawati. Segmentation echocardiography video using B-Spline and optical flow. In 2016 International Conference on Knowledge Creation and Intelligent Computing (KCIC), pages 226–231, Manado, Indonesia, November 2016. IEEE. ISBN 9781-5090-5231-8. doi: 10.1109/KCIC.2016.7883651. [54] Youssef Zinbi, Youssef Chahir, and Abder Elmoataz. Moving object Segmentation; using optical flow with active contour model. In 2008 3rd International Conference on Information and Communication Technologies: From Theory to Applications, pages 1–5, Damascus, Syria, April 2008. IEEE. ISBN 978-1-4244-1751-3. doi: 10.1109/ICTTA.2008.4530112. BIBLIOGRAF´ IA 65 [55] Yuemei Zhu, Yan Song, Xin Zhang, Pengfei Lv, Guangliang Li, Bo He, and Tianhong Yan. Segmentation of Underwater Object in Videos. In 2018 OCEANS - MTS/IEEE Kobe TechnoOceans (OTO), pages 1–4, Kobe, May 2018. IEEE. ISBN 978-1-5386-1654-3. doi: 10.1109/ OCEANSKOBE.2018.8559112. [56] Juhyoung Lee, Changhyeon Kim, Sungpill Choi, Dongjoo Shin, Sanghoon Kang, and Hoi-Jun Yoo. A 46.1 fps Global Matching Optical Flow Estimation Processor for Action Recognition in Mobile Devices. In 2018 IEEE International Symposium on Circuits and Systems (ISCAS), pages 1–5, Florence, May 2018. IEEE. ISBN 978-1-5386-4881-0. doi: 10.1109/ISCAS.2018. 8351177. [57] Guillermo Botella, Antonio Garcia, Manuel Rodriguez-Alvarez, Eduardo Ros, Uwe MeyerBaese, and Mar´ıa C. Molina. Robust Bioinspired Architecture for Optical-Flow Computation. IEEE Transactions on Very Large Scale Integration (VLSI) Systems, 18(4):616–629, April 2010. ISSN 1063-8210, 1557-9999. doi: 10.1109/TVLSI.2009.2013957. [58] Y. Li, X. Chen, and M. Yang. Optical flow based solar irradiance forecasting in satellite images. In 2019 IEEE International Conference on Real-Time Computing and Robotics (RCAR), pages 442–447, 2019. doi: 10.1109/RCAR47638.2019.9043950. [59] The MIT Press. Experiments in the Machine Interpretation of Visual Motion — The MIT Press. https://mitpress.mit.edu/books/experiments-machine-interpretation-visual-motion. (last accessed 2020-10-22). [60] Berthold Horn and B Schunck. “Determining optical flow”. Artificial Intelligence, 17(1-2): 185–203, 1981. ISSN 00043702. doi: 10.1016/0004-3702(93)90173-9. [61] M. I. Sereno and M. E. Sereno. Learning to discriminate senses of rotation and dilation with a Hebb rule. In Invest. Ophthal. Vis. Sci., Abstr, volume 31, page 528, 1990. [62] Vicki Bruce. Visual Perception : Physiology, Psychology, & Ecology. Psychology Press, Hove, [England] ;, 4th ed. edition, 2010. ISBN 0-203-42724-6. [63] Andrew B. Watson and Albert J. Ahumada. Model of human visual-motion sensing. JOSA A, 2(2):322–342, 1985. [64] David J. Heeger. Optical flow using spatiotemporal filters. International journal of computer vision, 1(4):279–302, 1988. [65] David J. Fleet and Allan D. Jepson. Computation of component image velocity from local phase information. International journal of computer vision, 5(1):77–104, 1990. [66] Ajit Singh. An estimation-theoretic framework for image-flow computation. In Image Understanding Workshop: Proceedings of a Workshop Held at Pittsburgh, Pennsylvania, September 11-13, 1990, page 314. Morgan Kaufmann Pub, 1990. [67] Padmanabhan Anandan. Measuring Visual Motion from Image Sequences. PhD thesis, University of Massachusetts Amherst, 1987. [68] C. Bandera and P.D. Scott. Foveal machine vision systems. In Conference Proceedings., IEEE International Conference on Systems, Man and Cybernetics, pages 596–599, Cambridge, MA, USA, 1989. IEEE. doi: 10.1109/ICSMC.1989.71367. [69] F. Robert and E. Dinet. An image filtering process based on foveal mechanism simulation. In Conference Record of the Thirty-First Asilomar Conference on Signals, Systems and Computers (Cat. No.97CB36136), volume 2, pages 1725–1729, Pacific Grove, CA, USA, 1997. IEEE Comput. Soc. ISBN 978-0-8186-8316-9. doi: 10.1109/ACSSC.1997.679197. 66 BIBLIOGRAF´ IA [70] Avago Technologies. Data Sheet: Optical sensor ADNS2610, September 2008. [71] ST Microelectronics. Data Sheet: Ultra-low-power Arm ® Cortex ® -M4 32-bit MCU+FPU, 100DMIPS, up to 1MB Flash, 128 KB SRAM, USB OTG FS, LCD, ext. SMPS, June 2019. [72] CCTV Surveillance. Elsevier, 1995. ISBN 978-0-7506-9028-7. doi: 10.1016/C2009-0-25247-0. [73] F. Leens. An introduction to I2C and SPI protocols. IEEE Instrumentation Measurement Magazine, 12(1):8–13, February 2009. ISSN 1941-0123. doi: 10.1109/MIM.2009.4762946. [74] Scott Campbell. Basics of the SPI Communication Protocol. https://www.circuitbasics.com/basics-of-the-spi-communication-protocol/, February 2016. (last accessed 2020-10-30). [75] DJI. DJI Phantom 4 Pro V2.0 - Drones profesionales - DJI. https://www.dji.com/es/phantom-4-pro-v2. (last accessed 2020-11-04). [76] ST Microelectronics. STEVAL-GMBL02V1. https://www.st.com/en/evaluationtools/steval-gmbl02v1.html. (last accessed 2020-11-04). [77] BaseCam. BaseCam SimpleBGC 32-bit : BaseCam Electronics. https://www.basecamelectronics.com/simplebgc32bit/. (last accessed 2020-11-04). [78] OlliW. STorM32-BGC Wiki. http://www.olliw.eu/storm32bgc-wiki/Main Page. (last accessed 2020-11-04). [79] Peter Moreton. Industrial Brushless Servomotors. Newnes Power Engineering Series. Newnes, Oxford, 2000. ISBN 1-281-05128-4. [80] Irving M. Gottlieb. Practical Electric Motor Handbook. Newnes, Oxford ;, 1997. ISBN 1-281-30864-1. [81] Piotr Wach. Brushless DC Motor Drives (BLDC), pages 281–380. Springer Berlin Heidelberg, Berlin, Heidelberg, 2011. ISBN 978-3-642-20221-6 978-3-642-20222-3. doi: 10.1007/978-3-642-20222-3 4. [82] ST Microelectronics. Data Sheet: Mainstream Performance line, Arm ® Cortex ® -M3 MCU with 256 Kbytes of Flash memory, 72 MHz CPU, motor control, USB and CAN, December 2018. [83] Texas Instruments. Data Sheet: DRV8313 2.5-A Triple 1/2-H BridgeDriver, October 2012. [84] Hong Cheng. Autonomous Intelligent Vehicles. Springer London, London, 2011. ISBN 978-1-4471-2279-1 978-1-4471-2280-7. doi: 10.1007/978-1-4471-2280-7. [85] Aboelmagd Noureldin, Tashfeen B. Karamat, and Jacques Georgy. Fundamentals of Inertial Navigation, Satellite-Based Positioning and Their Integration. Springer Berlin Heidelberg, Berlin, Heidelberg, 2013. ISBN 978-3-642-30465-1 978-3-642-30466-8. doi: 10.1007/ 978-3-642-30466-8. [86] TDK. MPU-6050 Six-Axis (Gyro + Accelerometer) MEMS MotionTracking  Devices. https://invensense.tdk.com/products/motion-tracking/6-axis/mpu-6050/. (last accessed 2020-11-08). [87] Haoran Wen. Toward Inertial-Navigation-on-Chip: The Physics and Performance Scaling of Multi-Degree-of-Freedom Resonant MEMS Gyroscopes. Springer Theses. Springer International Publishing, Cham, 2019. ISBN 978-3-030-25469-8 978-3-030-25470-4. doi: 10.1007/978-3-030-25470-4. BIBLIOGRAF´ IA 67 [88] Manon Kok, Jeroen D. Hol, and Thomas B. Sch¨on. Using inertial sensors for position and orientation estimation. Foundations and Trends ® in Signal Processing, 11(1-2):1–153, 2017. ISSN 1932-8346. doi: 10.1561/2000000094. [89] Nathaniel Pinckney. Pulse-width modulation for microcontroller servo control. IEEE potentials, 25(1):27–29, 2006. [90] Horizon Hobby. Specification for Spektrum Remote Receiver Interfacing. Enabling Use of Spektrum Remotes in Third-Party Products, April 2016. [91] ARM mbed. Futaba S-BUS controlled by mbed. https://os.mbed.com/users/Digixx/notebook/futabas-bus-controlled-by-mbed/, March 2012. (last accessed 2020-11-09). [92] MH and Ralf Helbing. Technical Specification Document: HoTT SUMD Data Protocol, December 2012. [93] Mathilde. Berchon. La Impresi´on 3D : Gu´ıa Definitiva Para Makers, Dise˜nadores, Estudiantes, Profesionales, Artistas y Manitas En General. Editorial Gustavo Gili, Barcelona, 2016. ISBN 84-252-2855-7. [94] Evan G. Hemingway and Oliver M. O’Reilly. Perspectives on Euler angle singularities, gimbal lock, and the orthogonality of applied forces and applied moments. Multibody System Dynamics, 44(1):31–56, September 2018. ISSN 1573-272X. doi: 10.1007/s11044-018-9620-0. [95] Su Whan Sung. Process Identification and PID Control. Process Identification and Proportional-Integral-Derivative Control. John Wiley, Singapore ;, 1st edition edition, 2009. ISBN 1-282-38214-4. doi: 10.1002/9780470824122. [96] Katsuhiko Ogata. Ingenier´ıa de Control Moderna. Pearson Educaci´on, Madrid, 4 ª ed. edition, 2003. ISBN 84-205-3678-4. [97] Rafael de la Rosa-Vidal, J. M. Guerrero-Rodriguez, and J. A. Lenero-Bardallo. Live Demonstration: A Tracking System Based on a Real-Time Bio-Inspired Optical Flow Sensor. In 2020 IEEE International Symposium on Circuits and Systems (ISCAS), pages 1–1, Sevilla, October 2020. IEEE. ISBN 978-1-72813-320-1. doi: 10.1109/ISCAS45731.2020.9181258. [98] Pavel Yosifovich. Windows Presentation Foundation 4.5 Cookbook. Packt Publishing, Birmingham, 2012. ISBN 9781283637404. [99] John Gossman. Introduction to Model/View/ViewModel pattern for building WPF apps. https://docs.microsoft.com/en-us/archive/blogs/johngossman/introduction-tomodelviewviewmodel-pattern-for-building-wpf-apps, August 2005. (last accessed 2020-1030). [100] Ashish Ghoda and Mamta Dalal. XAML Developer Reference. Pearson Education, December 2011. ISBN 978-0-7356-6809-6. [101] Anders Hejlsberg, Scott Wiltamuth, and Peter Golde. C# Language Specification. AddisonWesley Longman Publishing Co., Inc., USA, 2003. ISBN 978-0-321-15491-0. [102] Material Design In XAML Toolkit. http://materialdesigninxaml.net. (last accessed 2020-1013). [103] Microsoft Documentation. WriteableBitmap Class (System.Windows.Media.Imaging). https://docs.microsoft.com/enus/dotnet/api/system.windows.media.imaging.writeablebitmap. (last accessed 2020-10-14). 68 BIBLIOGRAF´ IA [104] Charles Petzold. Lines with Arrows. http://www.charlespetzold.com/blog/2007/04/191200.html. (last accessed 2020-10-14). [105] Juan A. Le˜nero-Bardallo, Ricardo Carmona-Gal´an, and ´ Angel Rodr´ıguez-V´azquez. A bioinspired vision sensor with dual operation and readout modes. Sensors Journal, IEEE, 16 (2):1–14, January 2016. ISSN 1530-437X. doi: 10.1109/JSEN.2015.2483898. [106] Juan A. Le˜nero-Bardallo, Ricardo Carmona-Gal´an, and ´ Angel Rodr´ıguez-V´azquez. A high dynamic range image sensor with linear response based on asynchronous event detection. In 22nd European Conference on Circuit Theory and Design, ECCTD 2015, pages 1–4, August 2015. doi: 10.1109/ECCTD.2015.7300079. [107] Juan A. Le˜nero-Bardallo, D.H. Bryn, and P. H¨afliger. Bio-inspired asynchronous pixel event tricolor vision sensor. Biomedical Circuits and Systems, IEEE Transactions on, 8(3):345–357, June 2014. ISSN 1932-4545. doi: 10.1109/TBCAS.2013.2271382. [108] Juan A. Le˜nero-Bardallo and P. H¨afliger. A dual operation mode bio-inspired vision sensor. In Biomedical Circuits and Systems Conference (BioCAS), 2013 IEEE, pages 310–313, October 2013. doi: 10.1109/BioCAS.2013.6679701. [109] J. A. Le˜nero-Bardallo, T. Serrano-Gotarredona, and Bernabe Linares-Barranco. A 3.6s latency asynchronous frame-free event-driven dynamic-vision-sensor. IEEE Journal of SolidState Circuits, 46(6):1443–1455, June 2011. [110] L. Farian, P. H¨afliger, and J. A. Le˜nero-Bardallo. A miniaturized two-axis ultra low latency and low-power sun sensor for attitude determination of micro space probes. IEEE Transactions on Circuits and Systems I: Regular Papers, 65(5):1543–1554, May 2018. ISSN 1549-8328. doi: 10.1109/TCSI.2017.2763990. [111] Juan A. Le˜nero-Bardallo, Jose M. Guerrero-Rodr´ıguez, Ricardo Carmona-Gal´an, and ´ Angel Rodr´ıguez-V´azquez. On the analysis and detection of flames with an asynchronous spiking image sensor. IEEE Sensors Journal, 18(16):6588–6595, 2018. doi: 10.1109/JSEN.2018. 2851063. [112] Juan A. Le˜nero-Bardallo, D.H. Bryn, and P. H¨afliger. Flame monitoring with an AER color vision sensor. In Circuits and Systems (ISCAS), 2013 IEEE International Symposium On, pages 2404–2407, May 2013. doi: 10.1109/ISCAS.2013.6572363. Anexos 69 Anexo A Rutina principal 1/* USER CODE BEGIN Header */ 2/** 3****************************************************************************** 4* @file : main.c 5* @brief : Main program body 6****************************************************************************** 7* @attention 8* 9* <h2><center>© Copyright (c) 2020 STMicroelectronics. 10 * All rights reserved.</center></h2> 11 * 12 * This software component is licensed by ST under BSD 3-Clause license, 13 * the "License"; You may not use this file except in compliance with the 14 * License. You may obtain a copy of the License at: 15 * opensource.org/licenses/BSD-3-Clause 16 * 17 ****************************************************************************** 18 */ 19 /* USER CODE END Header */ 20 /* Includes ------------------------------------------------------------------*/ 21 #include "main.h" 22 #include "spi.h" 23 #include "tim.h" 24 #include "usart.h" 25 #include "gpio.h" 26 27 /* Private includes ----------------------------------------------------------*/ 28 /* USER CODE BEGIN Includes */ 29 #include "eyes.h" 30 #include "gimbalControl.h" 31 /* USER CODE END Includes */ 32 33 /* Private typedef -----------------------------------------------------------*/ 34 /* USER CODE BEGIN PTD */ 35 36 /* USER CODE END PTD */ 37 38 /* Private define ------------------------------------------------------------*/ 39 /* USER CODE BEGIN PD */ 40 41 /* USER CODE END PD */ 42 43 /* Private macro -------------------------------------------------------------*/ 44 /* USER CODE BEGIN PM */ 45 71 72 46 /* USER CODE END PM */ 47 48 /* Private variables ---------------------------------------------------------*/ 49 50 /* USER CODE BEGIN PV */ 51 52 /* USER CODE END PV */ 53 54 /* Private function prototypes -----------------------------------------------*/ 55 void SystemClock_Config(void); 56 /* USER CODE BEGIN PFP */ 57 58 /* USER CODE END PFP */ 59 60 /* Private user code ---------------------------------------------------------*/ 61 /* USER CODE BEGIN 0 */ 62 63 /* USER CODE END 0 */ 64 65 /** 66 * @brief The application entry point. 67 * @retval int 68 */ 69 int main(void) 70 { 71 /* USER CODE BEGIN 1 */ 72 73 /* USER CODE END 1 */ 74 75 /* MCU Configuration--------------------------------------------------------*/ 76 77 /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ 78 79 LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SYSCFG); 80 LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_PWR); 81 82 NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4); 83 84 /* System interrupt init*/ 85 86 /* USER CODE BEGIN Init */ 87 88 /* USER CODE END Init */ 89 90 /* Configure the system clock */ 91 SystemClock_Config(); 92 93 /* USER CODE BEGIN SysInit */ 94 95 /* USER CODE END SysInit */ 96 97 /* Initialize all configured peripherals */ 98 MX_GPIO_Init(); 99 MX_USART2_UART_Init(); 100 MX_SPI2_Init(); 101 MX_TIM1_Init(); 102 MX_SPI3_Init(); 103 MX_TIM3_Init(); 104 MX_TIM4_Init(); 105 /* USER CODE BEGIN 2 */ 106 107 startupPrint(); 108 109 gimbalControlInit(); 110 ANEXO A. RUTINA PRINCIPAL 73 111 eyes_init(); 112 eyes_start(); 113 114 /* USER CODE END 2 */ 115 116 /* Infinite loop */ 117 /* USER CODE BEGIN WHILE */ 118 while (1) 119 { 120 /* USER CODE END WHILE */ 121 122 /* USER CODE BEGIN 3 */ 123 } 124 /* USER CODE END 3 */ 125 } 126 127 /** 128 * @brief System Clock Configuration 129 * @retval None 130 */ 131 void SystemClock_Config(void) 132 { 133 LL_FLASH_SetLatency(LL_FLASH_LATENCY_3); 134 while(LL_FLASH_GetLatency()!= LL_FLASH_LATENCY_3) 135 { 136 } 137 LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE1); 138 LL_RCC_HSI_Enable(); 139 140 /* Wait till HSI is ready */ 141 while(LL_RCC_HSI_IsReady() != 1) 142 { 143 144 } 145 LL_RCC_HSI_SetCalibTrimming(16); 146 LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI, LL_RCC_PLLM_DIV_1, 8, LL_RCC_PLLR_DIV_2); 147 LL_RCC_PLL_EnableDomain_SYS(); 148 LL_RCC_PLL_Enable(); 149 150 /* Wait till PLL is ready */ 151 while(LL_RCC_PLL_IsReady() != 1) 152 { 153 154 } 155 LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); 156 157 /* Wait till System clock is ready */ 158 while(LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) 159 { 160 161 } 162 LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); 163 LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); 164 LL_RCC_SetAPB2Prescaler(LL_RCC_APB2_DIV_1); 165 166 LL_Init1msTick(64000000); 167 168 LL_SetSystemCoreClock(64000000); 169 LL_RCC_SetUSARTClockSource(LL_RCC_USART2_CLKSOURCE_PCLK1); 170 } 171 172 /* USER CODE BEGIN 4 */ 173 174 /* USER CODE END 4 */ 175 80 B.2. SOURCE 150 // Write DR to send data through SPI 151 WRITE_REG(SPIx->DR, (value << 8) | (1U << 7 | reg)); 152 // Wait until RXNE is set 153 while(!(READ_BIT(SPIx->SR, SPI_SR_RXNE))); 154 READ_REG(SPIx->DR); 155 // Wait until end the current transaction 156 while((READ_BIT(SPIx->SR, SPI_SR_FTLVL)) | (READ_BIT(SPIx->SR, SPI_SR_FRLVL)) | (READ_BIT(SPIx->SR , SPI_SR_BSY))); 157 // Set again RX FIFO threshold adjusted to 8-bit word 158 SET_BIT(SPIx->CR2, SPI_CR2_FRXTH); 159 #else // HALF DUPLEX SPI MODE 160 161 #endif 162 } 163 /** 164 * @brief Receive a byte from ADNS2610 as reply of adns2610_sendByte(Device dev, uint8_t value) function 165 * @param dev Device address, it refers to SPI peripheral where the sensor is connected 166 * @param value Pointer to a variable where the received value is stored 167 */ 168 void adns2610_receiveByte(Device dev, uint8_t* value){ 169 170 GET_SPI_PERIPH(dev, SPIx); 171 172 #if FULL_DUPLEX_SPI 173 // Write DR to send data through SPI 174 WRITE_REG(*(__IO uint8_t*) &SPIx->DR, 0x00); 175 // Wait until RXNE is set 176 while(!(READ_BIT(SPIx->SR, SPI_SR_RXNE))); 177 *value = READ_REG(*(__IO uint8_t*) &SPIx->DR); 178 // Wait until end the current transaction 179 while((READ_BIT(SPIx->SR, SPI_SR_FTLVL)) | (READ_BIT(SPIx->SR, SPI_SR_FRLVL)) | (READ_BIT(SPIx->SR , SPI_SR_BSY))); 180 #else // HALF DUPLEX SPI MODE 181 182 #endif 183 } 184 /** 185 * @brief Send a byte to ADNS2610. It’s used to request to ADNS2610 a register value in IT mode 186 * @param dev Device address, it refers to SPI peripheral where the sensor is connected 187 * @param value Value of the sent value 188 */ 189 void adns2610_sendByte(Device dev, uint8_t value){ 190 191 GET_SPI_PERIPH(dev, SPIx); 192 193 #if FULL_DUPLEX_SPI 194 // Check TXE to send data 195 while(!(READ_BIT(SPIx->SR, SPI_SR_TXE))); 196 // Write DR to send data through SPI 197 WRITE_REG(*(__IO uint8_t*) &SPIx->DR, value); 198 // Wait until RXNE is set 199 while(!(READ_BIT(SPIx->SR, SPI_SR_RXNE))); 200 READ_REG(*(__IO uint8_t*) &SPIx->DR); 201 #else // HALF DUPLEX SPI MODE 202 203 #endif 204 } 205 /** 206 * @brief Read a frame from ADNS2610 by polling 207 * @param dev Device address, it refers to SPI peripheral where the sensor is connected 208 * @param buffer Array where the frame is going to be stored 209 */ 210 void adns2610_readFrame(Device dev, pixelTypeDef buffer[]){ 211 uint16_t idx = 0; ANEXO B. LIBRER´ IA PARA LA COMUNICACI´ ON CON EL SENSOR ADNS2610 81 212 213 while(idx < PIXEL_QTY){ 214 if(!idx){ 215 adns2610_writeRegister(dev, ADNS2610_PIXEL_DATA_REG, 0x01); 216 LL_mDelay(1); 217 buffer[idx] = adns2610_readRegister(dev, ADNS2610_PIXEL_DATA_REG); 218 219 if(buffer[idx] & (ADNS2610_PIXEL_VALID | ADNS2610_PIXEL_SOF)){ 220 idx++; 221 continue; 222 } 223 } 224 225 buffer[idx] = adns2610_readRegister(dev, ADNS2610_PIXEL_DATA_REG); 226 227 if(buffer[idx] & ADNS2610_PIXEL_SOF){ 228 idx = 0; 229 LL_mDelay(1); 230 continue; 231 } 232 233 if(buffer[idx] & ADNS2610_PIXEL_VALID){ 234 idx++; 235 } 236 } 237 } 238 /** 239 * @brief Check the status of a pixel 240 * @param Pixel The PIXEL DATA register value received from ADNS2610 241 * @return See PixelStatus 242 */ 243 PixelStatus adns2610_checkPixel(pixelTypeDef* Pixel){ 244 if(*Pixel & ADNS2610_PIXEL_VALID){ 245 if(*Pixel & ADNS2610_PIXEL_SOF){ 246 return VALID_SOF; 247 } 248 return VALID; 249 } 250 else if(*Pixel & ADNS2610_PIXEL_SOF){ 251 return NON_VALID_SOF; 252 } 253 else{ 254 return NON_VALID; 255 } 256 } 257 /** 258 * @brief Print the received frame values in the console through UART 259 * @param frame The array which contains the pixel values 260 */ 261 void adns2610_printImage(pixelTypeDef frame[]){ 262 uint16_t i = 0; 263 264 printf("=======================================================\r\n||"); 265 266 while(i < PIXEL_QTY){ 267 if(!(i % 18) & (i > 1)){ 268 printf("||\r\n||"); 269 } 270 printf(" %02d ", frame[i] & ADNS2610_PIXEL_DATA); 271 i++; 272 } 273 274 printf("||\r\n=======================================================\r\n"); 275 } 82 B.2. SOURCE Anexo C Librer´ıa para el c´alculo de flujo ´optico C.1. Header 1/* 2* opticalFlow.h 3* 4* Created on: 23 sept. 2020 5* Author: deros 6*/ 7 8#ifndef INC_OPTICALFLOW_H_ 9#define INC_OPTICALFLOW_H_ 10 11 #include <stdio.h> 12 #include "framesIndexers.h" 13 #include "adns2610.h" 14 15 /* Private constant ----------------------------------------------------------*/ 16 17 /* Exported typedefs ----------------------------------------------------------*/ 18 typedef struct __attribute__((__packed__)){ 19 int32_t x; 20 int32_t y; 21 } optical2DFlowStruct; 22 23 typedef struct __attribute__((__packed__)){ 24 int32_t x; 25 int32_t y; 26 int32_t theta; 27 } optical2DandRotateFlowStruct; 28 29 /* Exported functions --------------------------------------------------------*/ 30 void OF_ResetCoefficients(); 31 void OF_ComputeCoefficients(Device dev, uint8_t currentFrame[], uint8_t lastFrame[], int32_t idx); 32 void OF_Compute(Device dev, int32_t* ofX, int32_t* ofY); 33 void OF_ComputeFused(optical2DFlowStruct* right, optical2DFlowStruct* left, optical2DandRotateFlowStruct* fused); 34 35 #endif /* INC_OPTICALFLOW_H_ */ C.2. Source 83 84 C.2. SOURCE 1/* 2* opticalFlow.c 3* 4* Created on: 23 sept. 2020 5* Author: deros 6*/ 7 8#include "opticalFlow.h" 9 10 /* Private constants ----------------------------------------------------------*/ 11 #define int24_t_Mask 0x80FFFFFF 12 #define bitsOfResolution 9 13 14 static int64_t A[2]; 15 static int64_t B[2]; 16 static int64_t C[2]; 17 static int64_t D[2]; 18 static int64_t E[2]; 19 static int32_t deltaX; 20 static int32_t deltaY; 21 static int32_t deltaT; 22 static int16_t frameIdx; 23 24 /** 25 * @brief Reset the coefficient values and the frame’s pixel index to zero 26 */ 27 void OF_ResetCoefficients(){ 28 A[0] = B[0] = C[0] = D[0] = E[0] = 0; 29 A[1] = B[1] = C[1] = D[1] = E[1] = 0; 30 frameIdx = 0; 31 } 32 33 /** 34 * @brief It computes the optical flow coefficients related to a certain pixel if the needed data is available 35 * @param dev The device where the frames from 36 * @param currentFrame The current frame to process the optical flow 37 * @param lastFrame The reference frame to process the optical flow 38 * @param idx The frame’s pixel index which is going to be processed 39 */ 40 void OF_ComputeCoefficients(Device dev, uint8_t currentFrame[], uint8_t lastFrame[], int32_t idx){ 41 42 if(fSelect[idx]){ 43 deltaX = (lastFrame[f2[frameIdx]] & ADNS2610_PIXEL_DATA) - (lastFrame[f1[frameIdx]] & ADNS2610_PIXEL_DATA); 44 deltaY = (lastFrame[f4[frameIdx]] & ADNS2610_PIXEL_DATA) - (lastFrame[f3[frameIdx]] & ADNS2610_PIXEL_DATA); 45 deltaT = (currentFrame[f0[frameIdx]] & ADNS2610_PIXEL_DATA) - (lastFrame[f0[frameIdx]] & ADNS2610_PIXEL_DATA); 46 47 A[dev] += deltaX * deltaX; 48 B[dev] += deltaY * deltaX; 49 C[dev] += deltaT * deltaX; 50 D[dev] += deltaY * deltaY; 51 E[dev] += deltaT * deltaY; 52 53 frameIdx++; 54 } 55 } 56 57 /** 58 * @brief It computes the optical flow value from the coefficients computed in the last iterations 59 * @param dev The device where the frames from 60 * @param ofX Pointer to the variable where the optical flow value in X direction is going to be stored ANEXO C. LIBRER´ IA PARA EL C´ ALCULO DE FLUJO ´ OPTICO 85 61 * @param ofY Pointer to the variable where the optical flow value in Y direction is going to be stored 62 */ 63 void OF_Compute(Device dev, int32_t* ofX, int32_t* ofY){ 64 int64_t num, den; 65 66 den = A[dev] * D[dev] - B[dev] * B[dev]; 67 68 if(den > 0){ 69 num = (C[dev]*D[dev]) - (B[dev]*E[dev]); 70 *ofX = (num << bitsOfResolution) / den; 71 num = (A[dev]*E[dev]) - (B[dev]*C[dev]); 72 *ofY = (num << bitsOfResolution) / den; 73 } 74 else{ 75 *ofX = *ofY = 0; 76 } 77 } 78 79 /** 80 * @brief It computes the optical flow fusion from the optical flow values computed for the two connected devices 81 * @param right Pointer to the struct which contains the optical flow values computed from the right positioned device 82 * @param left Pointer to the struct which contains the optical flow values computed from the left positioned device 83 * @param fused Pointer to the struct where the fused values are going to be stored 84 */ 85 void OF_ComputeFused(optical2DFlowStruct* right, optical2DFlowStruct* left, optical2DandRotateFlowStruct* fused){ 86 fused->x = (right->x + left->x) >> 1; 87 fused->y = (right->y + left->y) >> 1; 88 if((right->y < 0 && left->y > 0) || (right->y > 0 && left->y < 0)) 89 fused->theta = (right->y - left->y); 90 } C.3. Header para almacenar la tabla de indices 1/* 2* framesIndexers.h 3* 4* Created on: 23 sept. 2020 5* Author: deros 6*/ 7 8#ifndef INC_FRAMESINDEXERS_H_ 9#define INC_FRAMESINDEXERS_H_ 10 11 #include "stdbool.h" 12 13 static const uint16_t f0[] = { 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, // 1 14 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, // 2 15 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, // 3 16 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, // 4 17 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, // 5 18 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, // 6 19 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, // 7 86 C.3. HEADER PARA ALMACENAR LA TABLA DE INDICES 20 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, // 8 21 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, // 9 22 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, // 10 23 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, // 11 24 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, // 12 25 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, // 13 26 253, 254, 255, 256, 257, 258, 259, 260, 261, 262, 263, 264, 265, 266, 267, 268, // 14 27 271, 272, 273, 274, 275, 276, 277, 278, 279, 280, 281, 282, 283, 284, 285, 286, // 15 28 289, 290, 291, 292, 293, 294, 295, 296, 297, 298, 299, 300, 301, 302, 303, 304 // 16 29 }; 30 31 static const uint16_t f1 [] = { 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, // 1 32 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, // 2 33 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, // 3 34 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, // 4 35 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, // 5 36 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, // 6 37 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, // 7 38 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, // 8 39 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, // 9 40 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, // 10 41 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, // 11 42 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, // 12 43 253, 254, 255, 256, 257, 258, 259, 260, 261, 262, 263, 264, 265, 266, 267, 268, // 13 44 271, 272, 273, 274, 275, 276, 277, 278, 279, 280, 281, 282, 283, 284, 285, 286, // 14 45 289, 290, 291, 292, 293, 294, 295, 296, 297, 298, 299, 300, 301, 302, 303, 304, // 15 46 307, 308, 309, 310, 311, 312, 313, 314, 315, 316, 317, 318, 319, 320, 321, 322 // 16 47 }; 48 49 static const uint16_t f2 [] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, // 1 50 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, // 2 51 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, // 3 52 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, // 4 53 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, // 5 54 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, ANEXO C. LIBRER´ IA PARA EL C´ ALCULO DE FLUJO ´ OPTICO 87 105, 106, // 6 55 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, // 7 56 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, // 8 57 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, // 9 58 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, // 10 59 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, // 11 60 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, // 12 61 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, // 13 62 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, // 14 63 253, 254, 255, 256, 257, 258, 259, 260, 261, 262, 263, 264, 265, 266, 267, 268, // 15 64 271, 272, 273, 274, 275, 276, 277, 278, 279, 280, 281, 282, 283, 284, 285, 286 // 16 65 }; 66 67 static const uint16_t f3 [] = { 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, // 1 68 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, // 2 69 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, // 3 70 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, // 4 71 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, // 5 72 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, // 6 73 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, // 7 74 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, // 8 75 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, // 9 76 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, // 10 77 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, // 11 78 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, // 12 79 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, // 13 80 254, 255, 256, 257, 258, 259, 260, 261, 262, 263, 264, 265, 266, 267, 268, 269, // 14 81 272, 273, 274, 275, 276, 277, 278, 279, 280, 281, 282, 283, 284, 285, 286, 287, // 15 82 290, 291, 292, 293, 294, 295, 296, 297, 298, 299, 300, 301, 302, 303, 304, 305 // 16 83 }; 84 85 static const uint16_t f4 [] = { 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, // 1 86 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, // 2 87 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, // 3 88 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, // 4 88 C.3. HEADER PARA ALMACENAR LA TABLA DE INDICES 89 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, // 5 90 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, // 6 91 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, // 7 92 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, // 8 93 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, // 9 94 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, // 10 95 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, // 11 96 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, // 12 97 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, // 13 98 252, 253, 254, 255, 256, 257, 258, 259, 260, 261, 262, 263, 264, 265, 266, 267, // 14 99 270, 271, 272, 273, 274, 275, 276, 277, 278, 279, 280, 281, 282, 283, 284, 285, // 15 100 288, 289, 290, 291, 292, 293, 294, 295, 296, 297, 298, 299, 300, 301, 302, 303 // 16 101 }; 102 103 104 105 static const bool fSelect[] = { false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, // 1 106 false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, // 2 107 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 3 108 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 4 109 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 5 110 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 6 111 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 7 112 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 8 113 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 9 114 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 10 115 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 11 116 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 12 117 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 13 118 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 14 119 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 15 120 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 16 121 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false, // 17 122 false, true, true, true, true, true, true, true, true, true, true, true , true, true, true, true, true, false // 18 123 }; ANEXO C. LIBRER´ IA PARA EL C´ ALCULO DE FLUJO ´ OPTICO 89 124 125 #endif /* INC_FRAMESINDEXERS_H_ */ 96 D.2. SOURCE 229 else{ 230 // It only waits for the next state when it is REQ_READING_FRAME. If not, it continues directly to PROCESSING state. 231 FSMstate = REQ_READING_FRAME; 232 eyes_waitIT(ADNS2610_TIM_BTW_RD); 233 collisionFlag = 0; 234 return; 235 } 236 #endif 237 collisionFlag = 0; 238 /* PROCESSING state ---------------------------------------------------------------- */ 239 case PROCESSING: 240 /* Check if it’s the first frame read */ 241 if(firstFrameRead){ 242 firstFrameRead = false; 243 } 244 else{ 245 /* Compute the Optical Flow from the previous computed coefficients */ 246 OF_Compute(ADNS2610_RIGHT, &(frames[currentFrameIdx].oFRight.x), &(frames[currentFrameIdx]. oFRight.y)); 247 #if SECOND_SENSOR_IMPLEMENTED 248 OF_Compute(ADNS2610_LEFT, &(frames[currentFrameIdx].oFLeft.x), &(frames[currentFrameIdx]. oFLeft.y)); 249 OF_ComputeFused(&frames[currentFrameIdx].oFRight, &frames[currentFrameIdx].oFLeft, &frames[ currentFrameIdx].oFFused); 250 #endif 251 } 252 /* Switch the frame structures to store the new frame in the "oldest" data buffer */ 253 SWITCH_FRAME_IDX(currentFrameIdx, lastFrameIdx); 254 FSMstate = TRIGGER_FRAME; 255 256 if(IsTrackingEnable()){ 257 applyControlLaw(frames[currentFrameIdx].oFFused.x, frames[currentFrameIdx].oFFused.y, frames[ currentFrameIdx].oFFused.theta); 258 eyes_waitControlTIM_IT(TIME_TO_POSITION); 259 } 260 else{ 261 eyes_waitIT(ADNS2610_TIM_BTW_RD); 262 } 263 return; 264 } 265 266 // Check for collisions between interrupts callings 267 collisionError: 268 printf("COLISSION ERROR!!\r\n"); 269 eyes_stopWaitIT(); 270 while(1); 271 } 272 273 /** 274 * It sets up a TIMER to wait the required times by the ADNS2610 sensor through an interrupt. 275 * The TIMER pre-scaler is configured to increase its count each 250ns. 276 */ 277 void eyes_configureFSM_TIM(void){ 278 // TIM1 prescalers has been configured to count microseconds 279 uint32_t temp = TIM1->CR1; 280 281 // Disable update interrupt 282 CLEAR_BIT(TIM1->DIER, TIM_DIER_UIE); 283 // Modify CR1 register 284 MODIFY_REG(temp, ~(TIM_CR1_UDIS), TIM_CR1_URS); 285 TIM1->CR1 = temp; 286 // Set interrupt interval 287 TIM1->ARR = ADNS2610_TIM_TO_RD; 288 // Update the prescaler and counter registers ANEXO D. LIBRER´ IA PARA EL DESARROLLO DE LA M´ AQUINA DE ESTADOS FINITOS IMPLEMENTADA 97 289 SET_BIT(TIM1->EGR, TIM_EGR_UG); 290 // Clear pending interrupt flag 291 CLEAR_BIT(TIM1->SR, TIM_SR_UIF); 292 // Enable update interrupt generation 293 CLEAR_BIT(TIM1->CR1, TIM_CR1_URS); 294 // Enable update interrupt 295 SET_BIT(TIM1->DIER, TIM_DIER_UIE); 296 // Configure NVIC to handle TIM1 update interrupt 297 NVIC_SetPriority(TIM1_UP_TIM16_IRQn, 1); 298 NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn); 299 } 300 301 /** 302 * It sets up the TIMER interval and it starts the count until the interrupt launch. 303 * @param Count250ns The interval to wait expressed as 250ns multiples 304 */ 305 void eyes_waitIT(uint32_t Count250ns){ 306 // Disable update interrupt generation 307 SET_BIT(TIM1->CR1, TIM_CR1_URS); 308 // Set time to wait 309 TIM1->ARR = Count250ns; 310 // Update the prescaler and counter registers 311 SET_BIT(TIM1->EGR, TIM_EGR_UG); 312 // Enable update interrupt generation 313 CLEAR_BIT(TIM1->CR1, TIM_CR1_URS); 314 // Enable and start timer 315 SET_BIT(TIM1->CR1, TIM_CR1_CEN); 316 } 317 318 /** 319 * It stops the TIMER count to avoid the TIMER continues launching interrupts each configured interval time. 320 */ 321 void eyes_stopWaitIT(){ 322 // Disable and start timer 323 CLEAR_BIT(TIM1->CR1, TIM_CR1_CEN); 324 } 325 326 /** 327 * It sets up a TIMER to wait the required time to move the platform to the new position through an interrupt. 328 * The TIMER pre-scaler is configured to increase its count each millisecond. 329 */ 330 void eyes_configureControl_TIM(void){ 331 // TIM1 prescalers has been configured to count microseconds 332 uint32_t temp = TIM4->CR1; 333 334 // Disable update interrupt 335 CLEAR_BIT(TIM4->DIER, TIM_DIER_UIE); 336 // Modify CR1 register 337 MODIFY_REG(temp, ~(TIM_CR1_UDIS), TIM_CR1_URS); 338 TIM4->CR1 = temp; 339 // Set interrupt interval 340 TIM4->ARR = 1; 341 // Update the prescaler and counter registers 342 SET_BIT(TIM4->EGR, TIM_EGR_UG); 343 // Clear pending interrupt flag 344 CLEAR_BIT(TIM4->SR, TIM_SR_UIF); 345 // Enable update interrupt generation 346 CLEAR_BIT(TIM4->CR1, TIM_CR1_URS); 347 // Enable update interrupt 348 SET_BIT(TIM4->DIER, TIM_DIER_UIE); 349 // Configure NVIC to handle TIM1 update interrupt 350 NVIC_SetPriority(TIM4_IRQn, 1); 351 NVIC_EnableIRQ(TIM4_IRQn); 98 D.2. SOURCE 352 } 353 354 /** 355 * It sets up the TIMER interval and it starts the count until the interrupt launch 356 * @param millis The interval to wait expressed in milliseconds 357 */ 358 void eyes_waitControlTIM_IT(uint32_t millis){ 359 // Disable update interrupt generation 360 SET_BIT(TIM4->CR1, TIM_CR1_URS); 361 // Set time to wait 362 TIM4->ARR = millis; 363 // Update the prescaler and counter registers 364 SET_BIT(TIM4->EGR, TIM_EGR_UG); 365 // Enable update interrupt generation 366 CLEAR_BIT(TIM4->CR1, TIM_CR1_URS); 367 // Enable and start timer 368 SET_BIT(TIM4->CR1, TIM_CR1_CEN); 369 } 370 371 /** 372 * It stops the TIMER count to avoid the TIMER continues launching interrupts each configured interval time. 373 */ 374 void eyes_stopWaitControlTIM_IT(){ 375 // Disable and start timer 376 CLEAR_BIT(TIM4->CR1, TIM_CR1_CEN); 377 } 378 379 /** 380 * It computes the next acquired pixel index: 381 * - If the status is good, the index is increased by one. 382 * - If global fault is detected the index is reset to zero. 383 * - If local fault is detected the index isn’t increased. 384 * @param status1 Pointer to pixel status type from one of the two devices 385 * @param status2 Pointer to pixel status type pixel status from one of the two devices 386 * @param idx1 Pointer to index for the next acquired pixel 387 * @param idx2 Poniter to index for the next acquired pixel 388 * @return True if pixels status are good, false if not 389 */ 390 bool eyes_computeIdxFromStatus(PixelStatus* status1, PixelStatus* status2, uint16_t* idx1, uint16_t * idx2){ 391 392 if((*status1 == VALID_SOF) && (*idx1 == 0)){ 393 (*idx1)++; 394 } 395 else if((*status1 == VALID) && (*idx1 != 0) && (*idx1 < PIXEL_QTY-1)){ 396 (*idx1)++; 397 } 398 else if ((*status1 == VALID_SOF) && (*idx1 != 0)){ 399 *idx1 = *idx2 = 0; 400 return false; 401 } 402 #if SECOND_SENSOR_IMPLEMENTED 403 if((*status2 == VALID_SOF) && (*idx2 == 0)){ 404 (*idx2)++; 405 } 406 else if((*status2 == VALID) && (*idx2 != 0) && (*idx2 < PIXEL_QTY-1)){ 407 (*idx2)++; 408 } 409 else if((*status2 == VALID_SOF) && (*idx2 != 0)){ 410 (*idx1) = (*idx2) = 0; 411 return false; 412 } 413 #endif 414 return true; ANEXO D. LIBRER´ IA PARA EL DESARROLLO DE LA M´ AQUINA DE ESTADOS FINITOS IMPLEMENTADA 99 415 } 416 417 void TIM1_UP_TIM16_IRQHandler(void){ 418 // If the interrupt flag is enabled 419 if(READ_BIT(TIM1->SR, TIM_SR_UIF)){ 420 // Clear pending interrupt flag 421 CLEAR_BIT(TIM1->SR, TIM_SR_UIF); 422 // Process FSM 423 eyes_FSM(); 424 } 425 } 426 427 void TIM4_IRQHandler(void){ 428 // If the interrupt flag is enabled 429 if(READ_BIT(TIM4->SR, TIM_SR_UIF)){ 430 // Clear pending interrupt flag 431 CLEAR_BIT(TIM4->SR, TIM_SR_UIF); 432 // Process FSM 433 eyes_FSM(); 434 } 435 } 100 D.2. SOURCE Anexo E Librer´ıa para la implementaci´on del control de la plataforma E.1. Header 1/* 2* gimbalControl.h 3* 4* Created on: 27 oct. 2020 5* Author: deros 6*/ 7 8#ifndef INC_GIMBALCONTROL_H_ 9#define INC_GIMBALCONTROL_H_ 10 11 /* Includes ------------------------------------------------------------------*/ 12 #include "usart.h" 13 #include "string.h" 14 #include <stdlib.h> 15 16 /* Exported functions --------------------------------------------------------*/ 17 void gimbalControlInit(void); 18 void applyControlLaw(int x, int y, int rotation); 19 bool IsTrackingEnable(); 20 21 #endif /* INC_GIMBALCONTROL_H_ */ E.2. Source 1/* 2* gimbalControl.c 3* 4* Created on: 27 oct. 2020 5* Author: deros 6*/ 7 8/* Includes ------------------------------------------------------------------*/ 9#include "gimbalControl.h" 10 11 /* Private macro -------------------------------------------------------------*/ 12 #define TAIL_CHAR ’\n’ 13 #define BUFFER_SIZE 10 14 101 102 E.2. SOURCE 15 /* DC PWM default and range values in CNT format*/ 16 #define MIN_POS 3199 // 1 ms 17 #define CENTER_POS 4799 // 1.5 ms 18 #define MAX_POS 6399 // 2 ms 19 #define DELTA_POS 50 20 21 /* PID parameters*/ 22 // PITCH 23 #define PITCH_P 0.45 24 #define PITCH_I 0.001 25 #define PITCH_D 0.001 26 // ROLL 27 #define ROLL_P 0.42 28 #define ROLL_I 0.001 29 #define ROLL_D 0.001 30 // YAW 31 #define YAW_P 0.55 32 #define YAW_I 0.001 33 #define YAW_D 0.001 34 35 #define DELTALIMIT 8 36 37 #define PITCH_WINDUP 500 38 #define ROLL_WINDUP 500 39 #define YAW_WINDUP 500 40 41 /* Private typedefs --------------------------------------------*/ 42 typedef enum commandEnum{ 43 UP, 44 DOWN, 45 LEFT, 46 RIGHT, 47 ROTATE_LEFT, 48 ROTATE_RIGHT, 49 CENTER, 50 TRACKING_ON, 51 TRACKING_OFF, 52 NA 53 } cmdTypeDef; 54 55 typedef struct{ 56 uint16_t pitchPos; 57 uint16_t rollPos; 58 uint16_t yawPos; 59 }motorPosTypeDef; 60 61 /* Private variables -------------------------------------------*/ 62 motorPosTypeDef motorPos = { .pitchPos = CENTER_POS, .rollPos = CENTER_POS, .yawPos = CENTER_POS}; 63 bool pwmEn; 64 bool trackingEn; 65 66 static int xSum = 0, ySum = 0, rotationSum = 0; 67 static int xLast = 0, yLast = 0, rotationLast = 0; 68 69 /* Private functions -------------------------------------------*/ 70 cmdTypeDef decodeCmd(char const * cmdString, int length); 71 void enablePWM(); 72 void disablePWM(); 73 __STATIC_INLINE void NormalizeRange(int value, int MaxRange, int MinRange); 74 75 /** 76 * @brief Setting up all the peripherals (UART and TIMER) needed 77 * to control the gimbal position 78 */ 79 void gimbalControlInit(void){ ANEXO E. LIBRER´ IA PARA LA IMPLEMENTACI´ ON DEL CONTROL DE LA PLATAFORMA 103 80 // Configure UART2 interrupt to receive data from PC 81 configure_IRQ_USART_RX(); 82 83 // Flag to know PWM signal state 84 pwmEn = false; 85 86 // Flag to know if tracking function is enable/disable 87 trackingEn = false; 88 } 89 90 /* -------------------------------------------------- 91 * PWM OUTPUTS: 92 * - RC-1: Pith --> TIM3->CH1 --> PA7 93 * - RC-2: Roll --> TIM3->CH2 --> PA6 94 * - RC-3: Yaw --> TIM3->CH4 --> PB1 95 * -------------------------------------------------- 96 */ 97 /** 98 * @brief Receive a string and decode the command type related to it 99 * @param cmdString The command in string format 100 * @param length The length of the command 101 * @return The command type in cmdTypeDef format 102 */ 103 cmdTypeDef decodeCmd(char const * cmdString, int length){ 104 105 // Enable PWM if it was disabled 106 if(!pwmEn) enablePWM(); 107 108 // Tracking enable command 109 if(strncmp(cmdString, "TRON\n", length) == 0){ 110 trackingEn = true; 111 return TRACKING_ON; 112 } 113 if(strncmp(cmdString, "TROFF\n", length) == 0){ 114 trackingEn = false; 115 116 return TRACKING_OFF; 117 } 118 119 // Tracking enable so It isn’t able to perform any command 120 if(trackingEn) return NA; 121 122 // Center command 123 if(strncmp(cmdString, "CN\n", length) == 0){ 124 125 motorPos.pitchPos = CENTER_POS; 126 motorPos.rollPos = CENTER_POS; 127 motorPos.yawPos = CENTER_POS; 128 129 TIM3->CCR1 = motorPos.pitchPos; 130 TIM3->CCR2 = motorPos.rollPos; 131 TIM3->CCR4 = motorPos.yawPos; 132 133 return CENTER; 134 } 135 136 // Up command 137 if(strncmp(cmdString, "UP\n", length) == 0){ 138 motorPos.pitchPos -= DELTA_POS; 139 NormalizeRange(motorPos.pitchPos, MAX_POS, MIN_POS); 140 TIM3->CCR2 = motorPos.pitchPos; 141 return UP; 142 } 143 // Down command 144 if(strncmp(cmdString, "DW\n", length) == 0){ 104 E.2. SOURCE 145 motorPos.pitchPos += DELTA_POS; 146 NormalizeRange(motorPos.pitchPos, MAX_POS, MIN_POS); 147 TIM3->CCR2 = motorPos.pitchPos; 148 return DOWN; 149 } 150 // Left command 151 if(strncmp(cmdString, "LF\n", length) == 0){ 152 motorPos.yawPos -= DELTA_POS; 153 NormalizeRange(motorPos.yawPos, MAX_POS, MIN_POS); 154 TIM3->CCR4 = motorPos.yawPos; 155 return LEFT; 156 } 157 // Right command 158 if(strncmp(cmdString, "RH\n", length) == 0){ 159 motorPos.yawPos += DELTA_POS; 160 NormalizeRange(motorPos.yawPos, MAX_POS, MIN_POS); 161 TIM3->CCR4 = motorPos.yawPos; 162 return RIGHT; 163 } 164 165 // Rotate left command 166 if(strncmp(cmdString, "RLF\n", length) == 0){ 167 motorPos.rollPos += DELTA_POS; 168 NormalizeRange(motorPos.rollPos, MAX_POS, MIN_POS); 169 TIM3->CCR1 = motorPos.rollPos; 170 return ROTATE_LEFT; 171 } 172 // Rotate right command 173 if(strncmp(cmdString, "RRH\n", length) == 0){ 174 motorPos.rollPos -= DELTA_POS; 175 NormalizeRange(motorPos.rollPos, MAX_POS, MIN_POS); 176 TIM3->CCR1 = motorPos.rollPos; 177 return ROTATE_RIGHT; 178 } 179 return NA; 180 } 181 182 /** 183 * \brief Implements the control law from the optical flow received as 184 * arguments 185 * @param x Optical flow value in horizontal direction 186 * @param y Optical flow value in vertical direction 187 * @param rotation Optical flow value which indicates the rotation 188 */ 189 void applyControlLaw(int x, int y, int rotation){ 190 int deltaPitch, deltaRoll, deltaYaw; 191 192 if(!trackingEn) return; 193 194 // Integrate values 195 xSum += x; 196 ySum += y; 197 rotationSum += rotation; 198 199 // Integral windup 200 if(abs(ySum) > PITCH_WINDUP) ySum = 0; 201 if(abs(rotationSum) > ROLL_WINDUP) rotationSum = 0; 202 if(abs(xSum) > YAW_WINDUP) xSum = 0; 203 204 // PID implementation 205 deltaYaw = (float)(x * YAW_P) + (float)(xSum * YAW_I) + (float)((x - xLast) * YAW_D); 206 deltaPitch = y * PITCH_P + ySum * PITCH_I + (y - yLast) * PITCH_D; 207 deltaRoll = rotation * ROLL_P + rotationSum * ROLL_I + 208 (rotation - rotationLast) * ROLL_D; 209 ANEXO E. LIBRER´ IA PARA LA IMPLEMENTACI´ ON DEL CONTROL DE LA PLATAFORMA 105 210 // Avoid small changes in computed values 211 if(abs(deltaYaw)>=DELTALIMIT) motorPos.yawPos +=deltaYaw; 212 if(abs(deltaPitch)>=DELTALIMIT) motorPos.pitchPos +=deltaPitch; 213 if(abs(deltaRoll)>=DELTALIMIT) motorPos.rollPos +=deltaRoll; 214 215 // Check the signals are in the proper range 216 NormalizeRange(motorPos.pitchPos, MAX_POS, MIN_POS); 217 NormalizeRange(motorPos.rollPos, MAX_POS, MIN_POS); 218 NormalizeRange(motorPos.yawPos, MAX_POS, MIN_POS); 219 220 // RC control signals generation 221 TIM3->CCR1 = motorPos.rollPos; 222 TIM3->CCR2 = motorPos.pitchPos; 223 TIM3->CCR4 = motorPos.yawPos; 224 225 // Save values to differentiation 226 xLast = x; 227 yLast = y; 228 rotationLast = rotation; 229 } 230 231 /** 232 * @brief Check if tracking function is enable/disable 233 * @return True if the tracking function is enable, False if it is disable 234 */ 235 bool IsTrackingEnable(){ 236 return trackingEn; 237 } 238 239 /** 240 * @brief Enable RC (PWM) signal generation 241 */ 242 void enablePWM(){ 243 // Enable output compare OCx channels 244 //SET_BIT(TIM3->CCER, TIM_CCER_CC4E); 245 MODIFY_REG(TIM3->CCER, ~(TIM_CCER_CC1NE | TIM_CCER_CC2NE), 246 (TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC4E)); 247 248 // Enable master output 249 MODIFY_REG(TIM3->BDTR, ~(TIM_BDTR_OSSI | TIM_BDTR_OSSR), TIM_BDTR_MOE); 250 251 // Enable counter 252 SET_BIT(TIM3->CR1, TIM_CR1_CEN); 253 254 pwmEn = true; 255 } 256 257 /** 258 * @brief Disable RC (PWM) signal generation 259 */ 260 void disablePWM(){ 261 // Disable output 262 //CLEAR_BIT(TIM3->CCER, TIM_CCER_CC4E); 263 CLEAR_BIT(TIM3->CCER, (TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC4E)); 264 265 // Disable master output 266 CLEAR_BIT(TIM3->BDTR, TIM_BDTR_MOE); 267 268 // Disable counter 269 CLEAR_BIT(TIM3->CR1, TIM_CR1_CEN); 270 271 // Reset motor position struct to initial values 272 motorPos.pitchPos = CENTER_POS; 273 motorPos.rollPos = CENTER_POS; 274 motorPos.yawPos = CENTER_POS; 112 24 <GroupBox.HeaderTemplate> 25 <DataTemplate> 26 <StackPanel Orientation="Horizontal"> 27 <materialDesign:PackIcon Kind="Eye" Height="32" Width="32" VerticalAlignment="Center" Margin="20 0 0 0"/> 28 <TextBlock Margin="8,15,0,15" VerticalAlignment="Center" Style="{StaticResource MaterialDesignSubtitle1TextBlock}" Text="{Binding}" /> 29 </StackPanel> 30 </DataTemplate> 31 </GroupBox.HeaderTemplate> 32 <Grid> 33 <Grid.RowDefinitions> 34 <RowDefinition Height="4*"/> 35 <RowDefinition Height="*"/> 36 </Grid.RowDefinitions> 37 <Grid.ColumnDefinitions> 38 <ColumnDefinition Width="*"/> 39 <ColumnDefinition Width="*"/> 40 </Grid.ColumnDefinitions> 41 <Image Grid.Row="0" Grid.ColumnSpan="2" Source="{Binding ImageSource.Source}" 42 Stretch="Uniform" Margin="3"> 43 </Image> 44 <Canvas x:Name="Canvas" Grid.Row="0" Grid.ColumnSpan="2" Visibility="{Binding OFEnable, Converter={StaticResource BooleanToVisibility}}"> 45 <petzold:ArrowLine x:Name="Arrow" 46 X1="{Binding ElementName=Canvas, Path=ActualWidth, Converter={StaticResource DimensionToCenter}}" 47 Y1="{Binding ElementName=Canvas, Path=ActualHeight, Converter={StaticResource DimensionToCenter}}" 48 ArrowEnds="End" StrokeThickness="3" Stroke="Red"> 49 <petzold:ArrowLine.X2> 50 <MultiBinding Converter="{StaticResource OpticalFlowToForwardPixels}" ConverterParameter="X"> 51 <Binding ElementName="Arrow" Path="X1"/> 52 <Binding ElementName="Arrow" Path="Y1"/> 53 <Binding Path="OFXFiltered"/> 54 <Binding Path="OFYFiltered"/> 55 </MultiBinding> 56 </petzold:ArrowLine.X2> 57 <petzold:ArrowLine.Y2> 58 <MultiBinding Converter="{StaticResource OpticalFlowToForwardPixels}" ConverterParameter="Y"> 59 <Binding ElementName="Arrow" Path="X1"/> 60 <Binding ElementName="Arrow" Path="Y1"/> 61 <Binding Path="OFXFiltered"/> 62 <Binding Path="OFYFiltered"/> 63 </MultiBinding> 64 </petzold:ArrowLine.Y2> 65 </petzold:ArrowLine> 66 <petzold:ArrowLine X1="{Binding ElementName=Canvas, Path=ActualWidth, Converter={StaticResource DimensionToCenter}}" 67 Y1="{Binding ElementName=Canvas, Path=ActualHeight, Converter={StaticResource DimensionToCenter}}" 68 ArrowEnds="End" StrokeThickness="3" Stroke="Green"> 69 <petzold:ArrowLine.X2> 70 <MultiBinding Converter="{StaticResource OpticalFlowToReversePixels}" ConverterParameter="X"> ANEXO G. DESCRIPCI´ ON EN XAML DE LA VISTA PARA MONITORIZACI´ ON DE LA IMAGEN Y EL FLUJO ´ OPTICO 113 71 <Binding ElementName="Arrow" Path="X1"/> 72 <Binding ElementName="Arrow" Path="Y1"/> 73 <Binding Path="OFXFiltered"/> 74 <Binding Path="OFYFiltered"/> 75 </MultiBinding> 76 </petzold:ArrowLine.X2> 77 <petzold:ArrowLine.Y2> 78 <MultiBinding Converter="{StaticResource OpticalFlowToReversePixels}" ConverterParameter="Y"> 79 <Binding ElementName="Arrow" Path="X1"/> 80 <Binding ElementName="Arrow" Path="Y1"/> 81 <Binding Path="OFXFiltered"/> 82 <Binding Path="OFYFiltered"/> 83 </MultiBinding> 84 </petzold:ArrowLine.Y2> 85 </petzold:ArrowLine> 86 </Canvas> 87 <GroupBox Grid.Row="1" Style="{DynamicResource MaterialDesignCardGroupBox}" Margin="10" UseLayoutRounding="False" SnapsToDevicePixels="True" 88 BorderThickness="0" materialDesign:ShadowAssist.ShadowDepth="Depth3" Padding="2" materialDesign:ColorZoneAssist.Mode="PrimaryLight" 89 Visibility="{Binding OFEnable, Converter={StaticResource BooleanToVisibility}}"> 90 <GroupBox.HeaderTemplate> 91 <DataTemplate> 92 <TextBlock Margin="5" VerticalAlignment="Center" HorizontalAlignment="Center" 93 Style="{StaticResource MaterialDesignSubtitle2TextBlock}" Text="Optical Flow in X" /> 94 </DataTemplate> 95 </GroupBox.HeaderTemplate> 96 <TextBlock Style="{StaticResource MaterialDesignSubtitle2TextBlock}" Text="{Binding OFX}" VerticalAlignment="Center" 97 HorizontalAlignment="Center"/> 98 </GroupBox> 99 <GroupBox Grid.Row="1" Grid.Column="1" Style="{DynamicResource MaterialDesignCardGroupBox}" Margin="10" UseLayoutRounding="False" SnapsToDevicePixels="True" 100 BorderThickness="0" materialDesign:ShadowAssist.ShadowDepth="Depth3" Padding="2" materialDesign:ColorZoneAssist.Mode="PrimaryLight" 101 Visibility="{Binding OFEnable, Converter={StaticResource BooleanToVisibility}}"> 102 <GroupBox.HeaderTemplate> 103 <DataTemplate> 104 <TextBlock Margin="5" VerticalAlignment="Center" HorizontalAlignment="Center" 105 Style="{StaticResource MaterialDesignSubtitle2TextBlock}" Text="Optical Flow in Y" /> 106 </DataTemplate> 107 </GroupBox.HeaderTemplate> 108 <TextBlock Style="{StaticResource MaterialDesignSubtitle2TextBlock}" Text="{Binding OFY}" VerticalAlignment="Center" 109 HorizontalAlignment="Center"/> 110 </GroupBox> 111 </Grid> 112 </GroupBox> 113 </Grid> 114 </UserControl> 114 Anexo H ‘View Model’ de la aplicaci´on ‘Eyes OF Gimbal Platform’ 1using System; 2using System.ComponentModel; 3using System.Diagnostics; 4using System.IO.Ports; 5using System.Threading; 6using System.Threading.Tasks; 7using System.Windows; 8using System.Windows.Media.Imaging; 9using FrameWrapper; 10 using FrameWrapper.Wrappers; 11 using OF_gimbal_platform.Model; 12 using OFGimbalPlatform.Base; 13 using OFGimbalPlatform.Model; 14 15 namespace OFGimbalPlatform.VM 16 { 17 public class ViewModel : BindableBase 18 { 19 /* Private data --------------------------------------------------------- */ 20 private SerialPort COM; 21 private readonly ADNS2610_wrapper<ADNS2610_Packet> adns2610Provider; 22 private SerialSTM32GimbalCommander commander; 23 24 /* Bindable data -------------------------------------------------------- */ 25 private BindableSensorData _sensorData; 26 public BindableSensorData SensorData 27 { 28 get { return _sensorData; } 29 set { SetProperty(ref _sensorData, value); } 30 } 31 private string _COM_sel; 32 public string COM_sel 33 { 34 get { return _COM_sel; } 35 set { 36 SetProperty(ref _COM_sel, value); 37 OnCOMSelectedChanged(); 38 } 39 } 40 private bool _StartIsEnable; 41 public bool StartIsEnable 42 { 115 116 43 get { return _StartIsEnable; } 44 set { SetProperty(ref _StartIsEnable, value); } 45 } 46 private bool _IsStarted; 47 public bool IsStarted 48 { 49 get { return _IsStarted; } 50 set { SetProperty(ref _IsStarted, value); } 51 } 52 private bool _IsOFEnable; 53 public bool IsOFEnable 54 { 55 get { return _IsOFEnable; } 56 set { SetProperty(ref _IsOFEnable, value); } 57 } 58 private bool _IsCalibrationMode; 59 public bool IsCalibrationMode 60 { 61 get { return _IsCalibrationMode; } 62 set { SetProperty(ref _IsCalibrationMode, value); } 63 } 64 private bool _IsTackingEnable; 65 public bool IsTrackingEnable 66 { 67 get { return _IsTackingEnable; } 68 set { SetProperty(ref _IsTackingEnable, value); } 69 } 70 71 /* Logic and tasks -------------------------------------------------------*/ 72 readonly CancellationTokenSource exitAppSource; 73 readonly CancellationToken exitAppToken; 74 readonly Task FrameUpdateTask; 75 readonly Task OFFilteringTask; 76 readonly SemaphoreSlim FrameUpdateSem; 77 78 /* UI commands -----------------------------------------------------------*/ 79 private DelegateCommand _exitCommand; 80 public DelegateCommand ExitCommand 81 { 82 get { return _exitCommand; } 83 set { SetProperty(ref _exitCommand, value); } 84 } 85 private DelegateCommand _StartCmd; 86 public DelegateCommand StartCmd 87 { 88 get { return _StartCmd; } 89 set { SetProperty(ref _StartCmd, value); } 90 } 91 private DelegateCommand _StopCmd; 92 public DelegateCommand StopCmd 93 { 94 get { return _StopCmd; } 95 set { SetProperty(ref _StopCmd, value); } 96 } 97 private DelegateCommand _OFEnableCmd; 98 public DelegateCommand OFEnableCmd 99 { 100 get { return _OFEnableCmd; } 101 set { SetProperty(ref _OFEnableCmd, value); } 102 } 103 private DelegateCommand _calibrationCmd; 104 public DelegateCommand CalibrationCmd 105 { 106 get { return _calibrationCmd; } 107 set { SetProperty(ref _calibrationCmd, value); } ANEXO H. ‘VIEW MODEL’ DE LA APLICACI´ ON ‘EYES OF GIMBAL PLATFORM’ 117 108 } 109 private DelegateCommand _TrackingEnableCmd; 110 public DelegateCommand TrackingEnableCmd 111 { 112 get { return _TrackingEnableCmd; } 113 set { SetProperty(ref _TrackingEnableCmd, value); } 114 } 115 private DelegateCommand _upGimbalCmd; 116 public DelegateCommand UpGimbalCmd 117 { 118 get { return _upGimbalCmd; } 119 set { SetProperty(ref _upGimbalCmd, value); } 120 } 121 private DelegateCommand _downGimbalCmd; 122 public DelegateCommand DownGimbalCmd 123 { 124 get { return _downGimbalCmd; } 125 set { SetProperty(ref _downGimbalCmd, value); } 126 } 127 private DelegateCommand _leftGimbalCmd; 128 public DelegateCommand LeftGimbalCmd 129 { 130 get { return _leftGimbalCmd; } 131 set { SetProperty(ref _leftGimbalCmd, value); } 132 } 133 private DelegateCommand _rightGimbalCmd; 134 public DelegateCommand RightGimbalCmd 135 { 136 get { return _rightGimbalCmd; } 137 set { SetProperty(ref _rightGimbalCmd, value); } 138 } 139 private DelegateCommand _rotateLeftGimbalCmd; 140 public DelegateCommand RotateLeftGimbalCmd 141 { 142 get { return _rotateLeftGimbalCmd; } 143 set { SetProperty(ref _rotateLeftGimbalCmd, value); } 144 } 145 private DelegateCommand _rotateRightGimbalCmd; 146 public DelegateCommand RotateRightGimbalCmd 147 { 148 get { return _rotateRightGimbalCmd; } 149 set { SetProperty(ref _rotateRightGimbalCmd, value); } 150 } 151 private DelegateCommand _centerGimbalCmd; 152 public DelegateCommand CenterGimbalCmd 153 { 154 get { return _centerGimbalCmd; } 155 set { SetProperty(ref _centerGimbalCmd, value); } 156 } 157 158 /// <summary> 159 /// View Model constructor 160 /// </summary> 161 public ViewModel() 162 { 163 #if DEBUG 164 /* Avoid the use of this class from code designer. This resolves the COM denied access problem. */ 165 if (DesignerProperties.GetIsInDesignMode(new DependencyObject())) return; 166 #endif 167 /* Data declaration ------------- */ 168 SensorData = new BindableSensorData(); 169 COM_sel = ""; 170 StartIsEnable = false; 171 IsStarted = false; 118 172 IsOFEnable = false; 173 IsCalibrationMode = false; 174 IsTrackingEnable = false; 175 176 adns2610Provider = new ADNS2610_wrapper<ADNS2610_Packet>(ADNS2610_Packet.PacketSize * 100, ADNS2610_Packet.Header, ADNS2610_Packet.PacketSize, true); 177 adns2610Provider.FrameAvailableEvent += ImageSource_FrameAvailableEvent; 178 179 /* Logic and tasks declaration --------- */ 180 exitAppSource = new CancellationTokenSource(); 181 exitAppToken = exitAppSource.Token; 182 FrameUpdateSem = new SemaphoreSlim(0, 10); 183 184 /* UI commands declaration ------------ */ 185 AssignDelegateCmd(); 186 187 /* Starting processes ------------------ */ 188 FrameUpdateTask = Task.Run(() => FrameUpdateTaskMethod(), exitAppToken); 189 OFFilteringTask = Task.Run(() => OFFilteringTaskMethod(), exitAppToken); 190 191 adns2610Provider.Start(); 192 } 193 194 #region Tasks 195 /// <summary> 196 /// In this task the frames detected in the input stream are updated in the GUI 197 /// </summary> 198 /// <returns>Task class to be managed by the app</returns> 199 private async Task FrameUpdateTaskMethod() 200 { 201 while (!exitAppToken.IsCancellationRequested) 202 { 203 await FrameUpdateSem.WaitAsync().ConfigureAwait(true); 204 205 if (exitAppToken.IsCancellationRequested) break; 206 207 await Application.Current.Dispatcher.BeginInvoke(new Action(() => 208 { 209 try 210 { 211 SensorData.GetRigthImageSource().Lock(); 212 SensorData.GetLeftImageSource().Lock(); 213 214 IntPtr pBackBufferRight = SensorData.GetRigthImageSource().BackBuffer; 215 IntPtr pBackBufferLeft = SensorData.GetLeftImageSource().BackBuffer; 216 217 unsafe 218 { 219 FillFrame(SensorData.GetRigthImageSource(), SensorData.GetRightBuffer()) ; 220 FillFrame(SensorData.GetLeftImageSource(), SensorData.GetLeftBuffer()); 221 } 222 223 SensorData.GetRigthImageSource().AddDirtyRect(new System.Windows.Int32Rect (0, 0, ADNS2610_Packet.FrameWidth, ADNS2610_Packet.FrameHeight)); 224 SensorData.GetLeftImageSource().AddDirtyRect(new System.Windows.Int32Rect(0, 0, ADNS2610_Packet.FrameWidth, ADNS2610_Packet.FrameHeight)); 225 } 226 finally 227 { 228 SensorData.GetRigthImageSource().Unlock(); 229 SensorData.GetLeftImageSource().Unlock(); 230 } 231 })); 232 } ANEXO H. ‘VIEW MODEL’ DE LA APLICACI´ ON ‘EYES OF GIMBAL PLATFORM’ 119 233 } 234 /// <summary> 235 /// This task does the filtering processing of the optical flow values received 236 /// </summary> 237 /// <returns></returns> 238 private async Task OFFilteringTaskMethod() 239 { 240 while (!exitAppToken.IsCancellationRequested) 241 { 242 if(IsOFEnable) 243 SensorData.FilteringOF(0.1); 244 await Task.Delay(10); 245 } 246 } 247 #endregion 248 249 #region Delegates 250 /// <summary> 251 /// This function is called when there are data in the COM port created by the platform in the PC 252 /// </summary> 253 /// <param name="sender">Who is the caller of the event</param> 254 /// <param name="e">Some information about the data received</param> 255 private void OnCOMDataReceived(object sender, SerialDataReceivedEventArgs e) 256 { 257 int n = COM.BytesToRead; 258 byte[] temp = new byte[n]; 259 260 n = COM.Read(temp, 0, n); 261 262 adns2610Provider.AddBytes(temp, 0, n); 263 } 264 /// <summary> 265 /// This event is called when a dataset has been detected by the frame wrapper 266 /// </summary> 267 /// <param name="payload">The dataset detected</param> 268 private void ImageSource_FrameAvailableEvent(ADNS2610_Packet payload) 269 { 270 SensorData.Copy(payload); 271 272 FrameUpdateSem.Release(); 273 } 274 /// <summary> 275 /// It is executed when is detected a change in the COM box at the GUI 276 /// </summary> 277 private void OnCOMSelectedChanged() 278 { 279 if(!StartIsEnable) StartIsEnable = true; 280 } 281 /// <summary> 282 /// Manage the start command 283 /// </summary> 284 /// <param name="sender">Information about the event caller</param> 285 private void OnStartCmd(object sender) 286 { 287 if ((COM == null)) 288 { 289 COM = new SerialPort(COM_sel, 921600, Parity.None, 8, StopBits.One) 290 { 291 ReceivedBytesThreshold = ADNS2610_Packet.PacketSize 292 }; 293 294 commander = new SerialSTM32GimbalCommander(COM); 295 } 296 if (!COM.IsOpen) 120 297 { 298 COM.Open(); 299 COM.DiscardInBuffer(); 300 COM.DataReceived += OnCOMDataReceived; 301 IsStarted = true; 302 } 303 } 304 /// <summary> 305 /// Manage the stop command 306 /// </summary> 307 /// <param name="sender">Information about the event caller</param> 308 private void OnStopCmd(object sender) 309 { 310 if ((COM != null) && COM.IsOpen) 311 { 312 COM.Close(); 313 COM.DataReceived -= OnCOMDataReceived; 314 IsStarted = false; 315 IsOFEnable = false; 316 IsTrackingEnable = false; 317 } 318 } 319 /// <summary> 320 /// Manage the enable optical flow command 321 /// </summary> 322 /// <param name="sender">Information about the event caller</param> 323 private void OnOFEnable(object sender) 324 { 325 IsOFEnable ^= true; 326 } 327 /// <summary> 328 /// Manage the calibration function mode command 329 /// </summary> 330 /// <param name="sender">Information about the event caller</param> 331 private void OnCalibration(object sender) 332 { 333 IsCalibrationMode ^= true; 334 SensorData.SetCalibrationMode(IsCalibrationMode); 335 } 336 /// <summary> 337 /// Manage the tracking function mode command 338 /// </summary> 339 /// <param name="sender">Information about the event caller</param> 340 private void OnTrackingEnable(object sender) 341 { 342 IsTrackingEnable ^= true; 343 344 if (IsTrackingEnable) commander.SendCommand(CommandType.TRACKING_ON); 345 else commander.SendCommand(CommandType.TRACKING_OFF); 346 } 347 #region Gimbal Delegates 348 /// <summary> 349 /// Manage the UP command to move the platform 350 /// </summary> 351 /// <param name="sender">Information about the event caller</param> 352 private void OnUpGimbalCmd(object sender) 353 { 354 Debug.WriteLine("OnUpGimbalCmd"); 355 commander.SendCommand(CommandType.UP); 356 } 357 /// <summary> 358 /// Manage the DOWN command to move the platform 359 /// </summary> 360 /// <param name="sender">Information about the event caller</param> 361 private void OnDownGimbalCmd(object sender) ANEXO H. ‘VIEW MODEL’ DE LA APLICACI´ ON ‘EYES OF GIMBAL PLATFORM’ 121 362 { 363 Debug.WriteLine("OnDownGimbalCmd"); 364 commander.SendCommand(CommandType.DOWN); 365 } 366 /// <summary> 367 /// Manage the LEFT command to move the platform 368 /// </summary> 369 /// <param name="sender">Information about the event caller</param> 370 private void OnLeftGimbalCmd(object sender) 371 { 372 Debug.WriteLine("OnLeftGimbalCmd"); 373 commander.SendCommand(CommandType.LEFT); 374 } 375 /// <summary> 376 /// Manage the RIGHT command to move the platform 377 /// </summary> 378 /// <param name="sender">Information about the event caller</param> 379 private void OnRightGimbalCmd(object sender) 380 { 381 Debug.WriteLine("OnRightGimbalCmd"); 382 commander.SendCommand(CommandType.RIGHT); 383 } 384 /// <summary> 385 /// Manage the ROTATE LEFT command to move the platform 386 /// </summary> 387 /// <param name="sender">Information about the event caller</param> 388 private void OnRotateLeftGimbalCmd(object sender) 389 { 390 Debug.WriteLine("OnRotateLeftGimbalCmd"); 391 commander.SendCommand(CommandType.ROTATE_LEFT); 392 } 393 /// <summary> 394 /// Manage the ROTATE RIGHT command to move the platform 395 /// </summary> 396 /// <param name="sender">Information about the event caller</param> 397 private void OnRotateRightGimbalCmd(object sender) 398 { 399 Debug.WriteLine("OnRotateRightGimbalCmd"); 400 commander.SendCommand(CommandType.ROTATE_RIGHT); 401 } 402 /// <summary> 403 /// Manage the CENTER command to move the platform 404 /// </summary> 405 /// <param name="sender">Information about the event caller</param> 406 private void OnCenterGimbalCmd(object sender) 407 { 408 Debug.WriteLine("OnCenterGimbalCmd"); 409 commander.SendCommand(CommandType.CENTER); 410 } 411 #endregion 412 /// <summary> 413 /// Manage the EXIT command 414 /// </summary> 415 /// <param name="obj">Information about the event caller</param> 416 private void OnExit(object obj) 417 { 418 adns2610Provider.Stop(); 419 Debug.WriteLine("Image source stopped."); 420 COM?.Close(); 421 Debug.WriteLine("COM closed."); 422 } 423 #endregion 424 425 #region Auxiliar methods 426 /// <summary> 128 43 X = data.X; 44 Y = data.Y; 45 Theta = data.theta; 46 } 47 } 48 public class BindableOF2D : BindableBase 49 { 50 private int _X; 51 public int X 52 { 53 get { return _X; } 54 set { SetProperty(ref _X, value); } 55 } 56 57 private int _Y; 58 public int Y 59 { 60 get { return _Y; } 61 set { SetProperty(ref _Y, value); } 62 } 63 64 public BindableOF2D() 65 { 66 X = Y = 0; 67 } 68 69 public void Copy(OF2D data) 70 { 71 X = data.X; 72 Y = data.Y; 73 } 74 } 75 public class BindableOF2DDouble : BindableBase 76 { 77 private double _X; 78 public double X 79 { 80 get { return _X; } 81 set { SetProperty(ref _X, value); } 82 } 83 84 private double _Y; 85 public double Y 86 { 87 get { return _Y; } 88 set { SetProperty(ref _Y, value); } 89 } 90 91 public BindableOF2DDouble() 92 { 93 X = Y = 0; 94 } 95 96 public void Copy(OF2D data) 97 { 98 X = data.X; 99 Y = data.Y; 100 } 101 } 102 103 public class BindableSensorData : BindableBase 104 { 105 private BindableOF2D _OFRight; 106 public BindableOF2D OFRight 107 { ANEXO J. MODELO DE DATOS PARA EL M ´ ODULO EYEOF 129 108 get { return _OFRight; } 109 set { SetProperty(ref _OFRight, value); } 110 } 111 private BindableOF2D _OFLeft; 112 public BindableOF2D OFLeft 113 { 114 get { return _OFLeft; } 115 set { SetProperty(ref _OFLeft, value); } 116 } 117 private BindableOF2DandRotation _OFFussed; 118 public BindableOF2DandRotation OFFussed 119 { 120 get { return _OFFussed; } 121 set { SetProperty(ref _OFFussed, value); } 122 } 123 private BindableOF2DDouble _OFRightFiltered; 124 public BindableOF2DDouble OFRightFiltered 125 { 126 get { return _OFRightFiltered; } 127 set { SetProperty(ref _OFRightFiltered, value); } 128 } 129 private BindableOF2DDouble _OFLeftFiltered; 130 public BindableOF2DDouble OFLeftFiltered 131 { 132 get { return _OFLeftFiltered; } 133 set { SetProperty(ref _OFLeftFiltered, value); } 134 } 135 private int _SEQ; 136 public int SEQ 137 { 138 get { return _SEQ; } 139 set { SetProperty(ref _SEQ, value); } 140 } 141 private Image _imageRight; 142 public Image ImageRight 143 { 144 get { return _imageRight; } 145 set { SetProperty(ref _imageRight, value); } 146 } 147 private Image _imageLeft; 148 public Image ImageLeft 149 { 150 get { return _imageLeft; } 151 set { SetProperty(ref _imageLeft, value); } 152 } 153 private readonly WriteableBitmap ImageSourceRight; 154 private readonly WriteableBitmap ImageSourceLeft; 155 private readonly byte[] FrameRight; 156 private readonly byte[] FrameLeft; 157 private byte[] FrameRightRef; 158 private byte[] FrameLeftRef; 159 private readonly object locker; 160 private bool CalibrationModeEnabled; 161 162 #region Public methods 163 public BindableSensorData() 164 { 165 locker = new object(); 166 167 OFRight = new BindableOF2D(); 168 OFLeft = new BindableOF2D(); 169 OFFussed = new BindableOF2DandRotation(); 170 171 OFRightFiltered = new BindableOF2DDouble(); 172 OFLeftFiltered = new BindableOF2DDouble(); 130 173 174 FrameRight = new byte[ADNS2610_Packet.FrameSize]; 175 FrameLeft = new byte[ADNS2610_Packet.FrameSize]; 176 FrameRightRef = new byte[ADNS2610_Packet.FrameSize]; 177 FrameLeftRef = new byte[ADNS2610_Packet.FrameSize]; 178 179 ImageSourceRight = new WriteableBitmap(ADNS2610_Packet.FrameWidth, ADNS2610_Packet. FrameHeight, 96d, 96d, PixelFormats.Gray8, null); 180 ImageRight = new Image 181 { 182 Source = ImageSourceRight 183 }; 184 185 ImageSourceLeft = new WriteableBitmap(ADNS2610_Packet.FrameWidth, ADNS2610_Packet. FrameHeight, 96d, 96d, PixelFormats.Gray8, null); 186 ImageLeft = new Image 187 { 188 Source = ImageSourceLeft 189 }; 190 191 CalibrationModeEnabled = false; 192 } 193 public byte[] GetRightBuffer() 194 { 195 return FrameRight; 196 } 197 public byte[] GetLeftBuffer() 198 { 199 return FrameLeft; 200 } 201 public WriteableBitmap GetRigthImageSource() 202 { 203 return ImageSourceRight; 204 } 205 public WriteableBitmap GetLeftImageSource() 206 { 207 return ImageSourceLeft; 208 } 209 public void Copy(ADNS2610_Packet data) 210 { 211 if (data == null)return; 212 213 SEQ = data.SEQ; 214 215 lock (locker) 216 { 217 if (!CalibrationModeEnabled) 218 { 219 OFRight.Copy(data.OFRight); 220 OFLeft.Copy(data.OFLeft); 221 OFFussed.Copy(data.OFFused); 222 } 223 else 224 { 225 ComputeOFInCalibrationMode(data.FrameRight, data.FrameLeft); 226 } 227 228 Array.Copy(data.FrameRight, 0, FrameRight, 0, ADNS2610_Packet.FrameSize); 229 Array.Copy(data.FrameLeft, 0, FrameLeft, 0, ADNS2610_Packet.FrameSize); 230 } 231 } 232 public void FilteringOF(double alpha) 233 { 234 lock(locker){ 235 OFLeftFiltered.X = OFLeftFiltered.X + alpha * (OFLeft.X - OFLeftFiltered.X); ANEXO J. MODELO DE DATOS PARA EL M ´ ODULO EYEOF 131 236 OFLeftFiltered.Y = OFLeftFiltered.Y + alpha * (OFLeft.Y - OFLeftFiltered.Y); 237 238 OFRightFiltered.X = OFRightFiltered.X + alpha * (OFRight.X - OFRightFiltered.X); 239 OFRightFiltered.Y = OFRightFiltered.Y + alpha * (OFRight.Y - OFRightFiltered.Y); 240 } 241 } 242 public void SetCalibrationMode(bool state) 243 { 244 lock (locker) 245 { 246 CalibrationModeEnabled = state; 247 248 Array.Copy(FrameRight, 0, FrameRightRef, 0, ADNS2610_Packet.FrameSize); 249 Array.Copy(FrameLeft, 0, FrameLeftRef, 0, ADNS2610_Packet.FrameSize); 250 } 251 } 252 #endregion 253 #region Auxiliar methods 254 private void ComputeOFInCalibrationMode(byte[] frameRight, byte[] frameLeft) 255 { 256 ComputeOFTool(frameRight, frameLeft, FrameRightRef, FrameLeftRef, OFRight, OFLeft, OFFussed); 257 } 258 #endregion 259 } 260 }