domingo, 12 de febrero de 2012

AHRS 9DOF Razor IMU Parte 1

Mis esfuerzos no quise centrarlos en el desarrollo del AHRS. Tiré la casa por la ventana y me compré el de Sparkfun con 9 grados de libertad. Debo indicar que tube algún que otro problema. Al cargar el código no daba lecturas correctas. Leyendo mucho pude saber que este IMU de Sparkfun tiene varios modelos puesto que lo van actualizando a nivel hardware. Pues bien, en aquellos momentos habían sacado una nueva tarjeta en la que incluían otro gyro, esa tarjeta era la mía, pero el código no estaba actualizado.

Al final encontré un código, el que han desarrollado para la tarjeta Mongoose, que ya lo habían adaptado para el nuevo gyro. La tarjeta Mongoose es prácticamente igual que la de Sparkfun solo que además le han añadido un sensor de presión para tener un control de la altura. Y además sale a menor precio. De haberlo sabido antes me habría comprado esta tarjeta. La web de la tarjeta Mongoose es ckdevices y el código original puede descargarse en mongoose-9dof-imu. Sin embargo este código no nos funcionará tal cual con la tarjeta de Sparkfun, tuve que hacerle unas modificaciones como las de anular las lecturas del sensor de presión y alguna más para adaptarlo a Arduino 1.0. El código final que funciona es el siguiente:

 Archivo para Arduino 1.0: Mongoose9DOF

Sin embargo a este código le encuentro sus pegas. Tal vez sea porque no he hecho una adaptación perfecta del código. Cada ciclo toma las lecturas del gyro y acelerómetro pero las del magnetómetro las toma cada 5 ciclos. El tiempo de ejecución del código he calculado que está en torno a 8 ó 9 ms pero el ciclo que toma la lectura del magnetómetro aumenta hasta los 14 ó 15 ms. No sé si esto es normal, intuyo que no, como he dicho antes tal vez sea por una mala adaptación por mi parte del código. La cuestión es que quedan tan sólo unos 5 ms para el envío por puerto serie de los datos. Con el método que viene programado el envío de los tres ángulos aún haciéndolo a 115.200 bps ya desborda el tiempo de ciclo de 20 ms. Así que desarrollé un pequeño protocolo que me permitiera enviar los ángulos con menos caracteres. Este protocolo se activa mediante #define EnviarArduino 1 y elaboro un byte para la transmisión de los signos de los tres ángulos y dos bytes más para cada uno de los ángulos. En total, teniendo la misma precisión de 1 centésima de grado, envío los tres ángulos con un protocolo de 8 bytes. La forma original envía más o menos bytes en función del valor numérico de los ángulos y si tienen signo negativo y en el peor de los casos puede llegar a tener hasta 24 bytes. La reducción es importante y permite enviar más datos o bien bajar la velocidad del puerto serie. Por el momento no está implementado con esta forma reducida el envío de los datos de los sensores, en concreto puede ser de utilidad el envío del acelerómetro que tal vez permitiría saber si el cuadricóptero se está desplazando para poder dejarlo quieto en el aire. Cuando el proyecto esté más avanzado y llegue el momento haré este tipo de pruebas.

NOTA: recientemente he visto en la página de Sparkfun que ya tienen actualizado su código, además viene con una aplicación para Processing para ver visualmente los datos enviados. Preveo que puede estar más optimizado y no tener los problemas del tiempo de ejecución del Mongoose así que aunque por el momento puede servir este código con el tiempo estudiaré el nuevo y le implementaré mi protocolo de envío de datos.
Podéis ver el nuevo código con un mejor funcionamiento en AHRS 9DOF Razor IMU Parte 2

11 comentarios:

  1. Cualquier comentario, aporte o consulta es bienvenido.

    ResponderEliminar
  2. Hola!! que suerte haber encontrado un blog como este!! Actualmente estoy enfrascado en un proyecto practicamente identico a este y me estaba encontrado con algunos problemas que no sabia solucionar!!

    Te he mandado un mail con algunas dudas a resolver si fuera posible >.<

    ResponderEliminar
  3. Hola marcesve. Me alegro de que te sirva mi blog. Sobre el mail que dices que me has enviado no me ha llegado nada... Sin embargo puedes preguntarme lo que quieras aquí en el mismo blog, así se ve que está vivo y seguro que le servirá también a otros usuarios que pueden tener tus mismas dudas.
    Y si tus preguntas no se corresponden al tema específico tratado en las entradas siempre podré preparar un nuevo tema y publicarlo.

    ResponderEliminar
  4. Bueno, era un mail un poco extenso pero puedo resumirlo bastante. Basicamente decia que estoy haciendo un Quad de proyecto de fin de carrera y que tengo algunos problemas que no sabia solventar.

    Yo estoy utilizando un arduino UNO y una imu identica a la tuya (lo se porque en el firmware que tienes colgado tienes el mismo modelo seleccionado que yo!!) Por contra, como apenas se de programacion (en la uni no nos enseñan nada) tenia enormes problemas para leerla desde el arduino. Por ello utilice el codigo que desarollaste.

    No obstante los problemas comenzaron a venir despues:
    En mi bucle de control, para leer la señal de cada uno de los canales de la emisora utilizo la funcion pulsein() de arduino. Esto me define 4 pulseins mas un quinto debido a un sensor de ultrasonidos que tengo instalado.
    Por otro lado, controlo los variadores con la biblioteca "servo" enviandoles un numero de "grados" que definen la potencia que les pido. Hasta aqui todo bien, ahora viene lo chungo.

    La funcion pulsein() mide el intervalo de un pulso en microsegundos. Para ello, si le dices que mida los pulsos en HIGH, Arduino espera hasta leer una secuencia LOW-HIGH-LOW y te devuelve la longitud de ese HIGH. Pues bien, si ese HIGH no se llega a producir (porque tienes la emisora apagado, o se te ha caido al rio o mil cosas asi) Arduino se queda PARADO esperando que aparezca el pulso durante un segundo entero. Como tengo que leer cuatro canales, si me quedo sin emisora Arduino se queda 4 segundos esperando a que lleguen dichas señales!!! eso quiere decir que cada ciclo va a durar mas de 4 segundos!!

    Se supone que para evitar eso, Arduino permite introducir un tercer parametro en la funcion pulsein() conocido como timeout, que es el tiempo en microsegundos que estas dispuesto a esperar al pulso. Pues bien, despues de introducir unos timeouts de 20 milisegundos en todos mis pulseins y probarlo, funciono bastante bien, con la emisora apagada arduino ejecutaba los ciclos en 76 o 77 milisegundos, y con la emisora encendida en 37 o 38. La pega es que los motores no respondian!!! porque?? ni idea.

    Haciendo exhaustivas probatinas descubri que el comando write de la libreria servo parece entrar en conflicto con cierto valor del timeout de comando pulsein().

    Por otro lado, probe tu codigo para la lectura de la IMU y funciono perfectamente hasta que añadiendole cosas, en concreto los cuatro pulseins para medir lo que dice la emisora, me percato de que el bucle de la lectura de la imu se ejecuta una vez y despues ya no!!

    Francamente me siento derrotado, cuando pensaba que habia acabado lo jodido (diseño del bucle de control en matlab, medida de los momentos de inercia, respuesta de los motores etc) me doy cuenta de que la programacion es un infierno y que probablemente no tenga ni remotamente el nivel que necesitaria para sacar esto adelante.

    Como puedes ver, son muchas cosas que no son faciles de explicar ni tienen buena pinta. Por ello si me dejases un mail o algo del estilo para conversar largo y tendido te estaria enormemente agradecido. Y si pudieses echarle un ojo a mi codigo por si ves a simple vista algo incongruente seria el sumun!!

    ResponderEliminar
  5. Corrijo, no es que la lectura de la imu se ejecute solo una vez, es que el servo que estaba utilizando de "chivato" para ver si funcionaba la IMU parece tambien afectado por el mismo problema de los variadores. En el momento en el que elimino las lineas de codigo relativas a los pulsein() todo vuelve a funcionar normalmente.

    ResponderEliminar
  6. Hola marcesve.
    Mi email puedes verlo en mi perfil: cheyenneforoarduino@gmail.com
    Puedes escribirme allí y explicarme con detalle.
    Yo me olvidaría de usar pulsein() para leer la emisora, tal como te explico en esa entrada nunca te va a funcionar bien. De hecho sabrás que el tiempo de ciclo de una emisora es 20 milisegundos así que si el código en Arduino se implementa bien el tiempo de ciclo no puede ser 78 ms o 37 ms, estando la emisora encendida o apagada.
    Probablemente pulsein() y la librería servo usarán el mismo timer con lo que entrarán en conflicto y no funcionarán bien.
    También hay que pensar que las librerías nos facilitan el trabajo pero hay que saber qué hacen ya que en algunos casos pueden complicarnos. La librería servo creo que funciona con interrupciones y según mi experiencia (que tampoco es tan grande) las interrupciones son un problema cuando también estamos haciendo mediciones de tiempo. A mí no me funcionó bien un pequeño programa en el que utilizaba un sensor de ultrasonidos con pulsein() y posicionaba un servo con la librería. Debo decir que creo que hay quien lo usa y le funciona... Yo llevo la idea de olvidarme de la librería servo y que mi código no tenga interrupciones.

    ResponderEliminar
  7. Ok, creo que seguire tus pasos y le dare una vuelta mas de tuerca al proyecto evitando usar las librerias. Gracias otra vez.

    ResponderEliminar
  8. Te he enviado un mail de nuevo bastante extenso con un pequeño codigo para probar si se leer mi emisora, mira a ver si te ha llegado esta vez >.<

    ResponderEliminar
  9. Éste sí que me ha llegado, te respondo también por email.

    ResponderEliminar
  10. mil gracias por el tiempo dedicado a esto ..... la verdad tenia como tres meses o mas en eso de las radiofrecuencias ..... pero con todo lo que subiste me a quitado muchas dudas y de mas .... y ahora entiendo mejor lo de pid al igual que lo de rc ppm ..... enserio amigo mil gracias ..... yo se que eres alguien ocupado pero si lo llegas a leer mi comentario enviame un correo a luilliwin8@gmail.com y te enseñare como esta mi proyecto y nuevamente gracias .... saludos desde celaya, guanajuato, mexico.......

    ResponderEliminar