Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software Software de bajo nivel

Transcripción

1 Capítulo 4. Conceptualmente, se dividirá el software desarrollado en dos niveles de abstracción. Se llamará software de bajo nivel a aquél cuyo fin es conseguir el correcto funcionamiento de todos los dispositivos hardware implicados, así como las interfaces de comunicación usadas, de forma que ofrezca una base operativa sobre la que montar el programa desarrollado para implementar el algoritmo de estimación deseado. Este programa constituirá el software de alto nivel, haciendo uso de los recursos que aporta el software de bajo nivel. En primer lugar se describe el entorno de desarrollo que ha permitido desarrollar este software, y a continuación se describe el propio software, y su distribución en la memoria del DSP de la placa base Entorno de desarrollo Para depurar el diseño tanto hardware como software de la futura IMU, se construyó una primera placa de desarrollo para el DSP. Tras varias rectificaciones, se obtuvo la placa sobre la que poder desarrollar un software, y que sería una versión algo más grande y casera que la definitiva. Llamaremos a esta placa a partir de ahora placa de desarrollo, y forma parte del entorno de desarrollo que se describe a continuación. La construcción de esta placa se hizo bajo la premisa de que se comportara como la placa de desarrollo comercial que existe en el laboratorio. Esta placa se encuentra en el controlador del vehículo autónomo ROMEO-4R, e incluye un DSP de características parecidas al de nuestra IMU, el TMS320LF2407. Es decir, la idea era aprovechar las herramientas de programación disponibles para esta placa. 57

2 Ilustración 4-1. Placa de desarrollo La placa de desarrollo creada posee una conexión para el puerto JTAG del DSP. En este puerto se conecta el emulador XDS510PP, de Spectrum Digital, que ofrece una conexión vía puerto paralelo con un PC. El software usado en el PC para desarrollar el software del DSP es el paquete de Texas Instruments Code Composer Studio. Este programa permite escribir el código en lenguaje C y cargarlo en la memoria del DSP. Los pasos en este proceso de programación son estos: - Escritura del código en lenguaje C o en ensamblador. En nuestro caso, prácticamente todo el programa está escrito en C. - Compilación del código. El código escrito en C es automáticamente traducido al ensamblador del DSP gracias a la librería rts2xx.lib. - Lincado. El lincador enlaza los distintos módulos del programa y genera el fichero.out con el programa en código máquina listo para cargar en la memoria del DSP. - Carga en memoria. El programa se carga en la memoria del DSP. El programa está dividido en distintas secciones, algunas de ellas se graban en la memoria flash del dispositivo, y otras se cargan en RAM. Además del emulador conectado al puerto paralelo del PC, el puerto serie del mismo también se puede usar para establecer una comunicación con el DSP. Usando un adaptador basado en el chip MAX232, conectamos el puerto serie de la placa de desarrollo con el puerto serie RS232 del PC. Usando el programa HyperTerminal, 58

3 que incluye el propio Windows, se pudo establecer una interfaz interactiva en la que el DSP respondiera a diversos comandos introducidos con el teclado del ordenador. Aparte de la placa de desarrollo anteriormente mencionada, se construyeron otras dos placas, de cara a poder probar el software desarrollado, así como la viabilidad del diseño hardware realizado. Una de estas placas de prueba consiste en una versión reducida de la placa de sensores. En ella se incluyen un acelerómetro, un conversor analógico-digital, y un conector para el compás magnético, además de un conector para poder comunicarla con la placa de desarrollo. Ilustración 4-2. Placa de prueba de sensores Con la otra placa se consiguió, tras varios intentos fallidos, crear una interfaz de adaptación de niveles lógicos para la interconexión de señales digitales de distinta naturaleza: las de 5 voltios y las de 3.3 voltios. En la siguiente imagen se puede apreciar la placa de desarrollo conectada a la placa de prueba de sensores, a través de esta placa de traducción de niveles de tensión. Se puede ver también la placa base conectada al emulador a través del puerto JTAG, y al puerto serie del ordenador, usando la placa de conversión TTL RS

4 Ilustración 4-3. Entorno de desarrollo Ilustración 4-4. Detalle del hardware de desarrollo conectado al PC 4.2. Memoria A continuación se detalla el mapa de memoria del TMS320LF2406A. La memoria disponible se encuentra dividida en varias secciones: 60

5 - Memoria Flash: Desde la dirección 0x0000 hasta la dirección 0x7FFF de la memoria de programa. Se divide a su vez en cuatro bloques. - Memoria RAM: Consta de cuatro bloques: o Bloque B0: de tipo DARAM (dual-access-ram), de 256 palabras de 16 bits. Este bloque se puede mapear tanto en memoria de programa como en memoria de datos. o Bloque B1: de tipo DARAM, de 256 palabras en memoria de datos. o Bloque B2: 32 palabras de tipo DARAM. o Bloque SARAM: 2Kwords (2048 palabras de 16 bits) de memoria single-access-ram. Se encuentra mapeada en el espacio de memoria de programa o en el de memoria de datos, según el valor de un bit de un determinado registro del DSP. 61

6 Ilustración 4-5. Mapa de memoria del TMS320LF2406A Toda esta información de memoria hay que tenerla en cuenta a la hora de programar el DSP. Se debe ser consciente del tamaño disponible tanto para programa como para datos, y ajustar el código a estas condiciones. Para que todo esto encaje, el programa Code Composer Studio permite definir un fichero con las especificaciones de memoria. En él se definen las distintas áreas de memoria física de las que se disponen, y la distribución de las distintas secciones 62

7 de código en ellas. Para el DSP de la IMU se creó un fichero de este tipo con el siguiente contenido: /****************************************************************************/ /* LINKER COMMAND FILE - MEMORY SPECIFICATION for C240/243 */ /* */ /****************************************************************************/ MEMORY { } PAGE 0 : VECS : origin = 0h, length = 044h /* FLASH */ PROG : origin = 44h, length = 7fbch /* FLASH */ PAGE 1 : B2 : origin = 0060h, length = 020h /* DARAM */ B0 : origin = 0200h, length = 0100h /* DARAM */ B1 : origin = 0300h, length = 0100h /* DARAM */ SARAM : origin = 0800h, length = 0800h /* SARAM */ /* */ /* SECTIONS ALLOCATION */ /* */ SECTIONS { vectors : { } > VECS PAGE 0 /* INTERRUPT VECTOR TABLE */.text : { } > PROG PAGE 0 /* User's code */.bss : PAGE 1.cinit : { } > PROG PAGE 0.stack : { } > B0 PAGE 1.const : load = PROG PAGE 0, run = SARAM PAGE 1 { const_run =.; *(.c_mark) *(.const) const_length =.- const_run; } } Ilustración 4-6. Fichero de especificación de secciones de memoria En la memoria se definen dos páginas, la de programa y la de datos. Dentro de cada una de ellas se definen los distintos bloques de memoria disponibles. La sección que contiene los vectores de interrupción se mapea a partir de la dirección 0x0000, tal como le corresponde según el mapa de memoria. La sección.text contiene el código ejecutable y se sitúa en memoria no volátil dentro de la memoria de programa. La sección.bss reserva el espacio necesario para las variables, y se sitúa en memoria de datos RAM. La sección.cinit contiene tablas para la inicialización de variables y constantes. La sección.stack es la pila del programa, y se mapea en el bloque B0. La sección.const contiene constantes, y la inicialización de variables de tipo global o static. Esta sección se carga en memoria 63

8 no volátil, pero a la hora de iniciarse el sistema, se copia a RAM para que sea utilizada desde allí Programa bajo nivel Para que el sistema funcione correctamente y sea capaz de ejecutar el algoritmo deseado, así como hacer uso de los periféricos correspondientes de forma conveniente, es necesario disponer de rutinas software tanto de inicialización como de configuración del sistema, así como drivers que nos permitan utilizar los periféricos. La primera rutina en ser ejecutada es la de inicialización. En esta rutina se asignan los valores adecuados a los registros de configuración del sistema. Con esto se consigue habilitar los periféricos deseados, ajustar el reloj del sistema a 40 MHz y mapear la SARAM en la memoria de datos. También se anula el watchdog timer, para que no se resetee el DSP. Desde esta rutina también se llama a las rutinas de inicialización de los distintos periféricos utilizados, para que queden ya configurados. Al principio de la rutina de inicialización se enmascaran todas las interrupciones, y al finalizar dicha rutina se activan las interrupciones que se van a utilizar, de forma que el sistema queda listo para empezar su trabajo. La sección que se carga a partir de la primera dirección de memoria es la tabla de vectores de interrupción. Esta tabla está escrita directamente en ensamblador: cada instrucción es un salto incondicional a la dirección de memoria donde está definida la rutina de servicio de la interrupción correspondiente. El primer vector de interrupción es el de reset, y manda el contador de programa al objeto _c_int0, que se corresponde con el lugar en donde empieza el programa en C, es decir, la función main. Los seis vectores siguientes corresponden a las interrupciones hardware en sus seis niveles de prioridad, y todos apuntan a la misma rutina, la rutina de servicio primaria (gisr). Será en esta rutina de servicio primaria donde se distinga cuál ha sido la interrupción que ha saltado. La rutina de atención primaria consiste en una estructura selectiva tipo switch, en la que se llama a la función adecuada en función del vector PIVR, que contiene un código con la interrupción que ha sido atendida. En cuanto a los drivers, el programa dispone de drivers para la lectura de PWM, para la gestión del puerto SPI y para la lectura y escritura en el puerto SCI. El driver del SPI está configurado para hacer posible la comunicación con el conversor analógico-digital. En esta interfaz, el DSP, que actúa de maestro, pone el 64

9 reloj para el sincronismo en los datos, y se establece un protocolo en la comunicación. En primer lugar el DSP activa la salida de propósito general que corresponde al chip select del conversor de la placa correspondiente. A continuación manda un byte de control en el que se especifica el canal del que se quiere leer. El conversor contesta con otros dos bytes en los que incluye la palabra con el dato requerido. Por último, el DSP desactiva el chip select. Todo este procedimiento lo realiza el driver del SPI cuando se llama a la función de escritura-lectura por el puerto SPI. Esta comunicación es bastante rápida, puesto que el reloj se configura a 2.35 MHz, pero habrá que realizarla para la lectura de cada uno de los canales de cada uno de los conversores de las placas de sensores. Nótese que para esta comunicación no se requiere el uso de interrupciones, sino que se produce cuando se pide desde el programa principal. Para la lectura del PWM se hace uso del periférico Event Manager, que controla el uso de las entradas capturadotas de eventos. Tres de estas entradas se usan para leer la señal de PWM de las tres placas de sensores. Para este driver se hace uso de uno de los temporizadores del DSP, el Timer 3. El funcionamiento de este driver se basa en contar el tiempo transcurrido entre flanco de subida y de bajada en la señal PWM. El dato que se quiere obtener es proporcional al este tiempo. Cada vez que se detecta un flanco en la señal se genera una interrupción, y es en la rutina de atención a esta interrupción donde se realiza el cálculo. De esta forma se va actualizando el valor de la medida en una variable global, a la que el programa principal podrá acceder en cualquier momento. El puerto serie SCI se configura para la transmisión a 9600 baudios, con palabras de 8 bits y sin paridad. Se genera una interrupción cuando se recibe un dato, e inmediatamente se contesta con n bytes, en función del dato recibido. Éste es el procedimiento que se establece para pedir información a la IMU. Cuando se quiera saber, por ejemplo, los ángulos RPY, se enviará a la IMU el byte de control correspondiente, y la IMU contestará con los correspondientes bytes de datos. Todo lo explicado hasta ahora hace alusión a rutinas de configuración e inicialización del sistema, así como a funciones que responden a interrupciones o a una llamada software. Pero, una vez realizada la inicialización, cómo se estructura el código correspondiente al algoritmo principal de forma que se ejecute en tiempo real? Para solucionar esto se ha utilizado parte del software desarrollado para el control a bajo nivel del ROMEO-4R. Para él había sido desarrollado un planificador que utiliza uno de los timers del DSP para repartir en el tiempo las distintas tareas atendiendo a la prioridad de las mismas y a la cadencia deseada en la ejecución de cada una de ellas. Se ha adaptado este código a nuestro sistema, de forma que se puede definir el algoritmo de estimación de la orientación como una tarea cíclica que 65

10 el planificador se encarga de ejecutar cada 20 ms. El planificador permite añadir otras tareas, como las lecturas de los sensores, de forma que se ejecuten con la frecuencia deseada. La estructura global del programa de bajo nivel se divide en varios módulos, de la siguiente forma: - main.c: módulo que contiene la función main, en la que se comenzará la ejecución tras un reset del sistema. - inicia.c: contiene la rutina de inicialización, que es la primera a la que se llama desde la función main. - rut_int.c: aquí se encuentra la rutina de servicio maestra, desde la que se llama a las distintas rutinas de atención a interrupciones. También se encuentran en este módulo las rutinas de atención a las interrupciones de los temporizadores 1 y 3, y de la recepción de datos por el puerto serie SCI. - vectores.asm: el único módulo que está escrito en ensamblador. Contiene la tabla de vectores de interrupción. - scheduler.c: contiene las rutinas necesarias para el funcionamiento del planificador. - sci.c: driver del SCI. En este módulo se encuentra la rutina de inicialización del puerto SCI, así como la función para enviar datos por dicho puerto. Esta función es llamada tras la recepción de un comando válido por el puerto SCI, con la consecuente activación de la interrupción por recepción, para enviar los datos solicitados. - spi.c: el driver del puerto serie síncrono SPI está desarrollado únicamente para la comunicación del DSP con el conversor A/D. Aquí se incluyen la rutina de inicialización del puerto y la función que implementa la comunicación con el conversor A/D, que incluye tanto la escritura como la lectura por este puerto. - pwm.c: este módulo contiene la rutina de inicialización del driver de lectura de PWM y las rutinas de atención a las interrupciones generadas por las tres entradas de PWM. En ellas se calcula el ancho del pulso recibido y se escala convenientemente para que tome el sentido físico deseado. - trat_dat.c: todo el tratamiento de datos necesario para obtener los cuaterniones o ángulos RPY en función del vector de estado actual se incluye 66

11 en este módulo. También forman parte de él las funciones necesarias para transformar las medidas angulares de los compases magnéticos en componentes del vector magnético, así como las transformaciones necesarias en los datos obtenidos de las lecturas de los sensores. - tarea_lectura.c: Aquí se encuentra una de las funciones que serán tareas gestionadas por el planificador. Ésta se encargará de actualizar las medidas de los sensores. - tarea_algoritmo.c: El filtro de Kalman que va actualizando el vector de estado será otra de estas tareas. Las funciones que usa el algoritmo de estimación están aquí contenidas. También se incluyen varios módulos en ensamblador, que realizan funciones matemáticas como el seno, el coseno, la inversa y la raíz cuadrada. Aparte de estos ficheros, el proyecto de Code Composer contiene también los ficheros de cabecera.h con los prototipos de las funciones, y el fichero Mimapa_flash.cmd, que contiene la información necesaria para situar el programa en la memoria del DSP, y que se comentó en el apartado anterior. 67

12 68