Autor Tema: Giroscopos - UAV's  (Leído 9695 veces)

0 Usuarios y 1 Visitante están viendo este tema.

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Giroscopos - UAV's
« en: 05 de Enero de 2014, 11:46:29 »
Los giróscopos son sensores que miden la velocidad angular a la que es sometido. Si bien existen sensores que miden en uno o dos ejes, hoy en día son muy comunes los giróscopos que miden en los tres ejes.
En mi caso estoy trabajando y mostraré resultados aquí del L3G4200D de ST. Lo tengo montado una plaquita que compre en dx.com. Si bien posee interfaz SPI, en la placa esta cableado el bus I2C, para facilitar la lectura de los 4 sensores de la IMU.
Esta es la página del producto. En ella podrán encontrar la hoja de datos así como algunas notas de aplicacion.

La gran mayoría de los sensores de tecnologia MEMS (en realidad la mayoria de todos los sensores) poseen 3 parámetros fundamentales:
El Offset (Zero rate level): Cuando el giróscopo esta en "quieto" debería entregar en su salida 0 °/seg. Sin embargo no es así y con el giróscopo en reposo siempre tendremos un valor distinto de 0. En la hoja de datos del L3G, en la Tabla 4, podemos ver que dependiendo de la sensibilidad elegida tenemos distintos Zero rate level, por ejemplo con un rango de 2000dps (°/seg) tenemos un offset típico de +-75dps. El valor de un sensor soldado en una placa y listo para usar es batanta fijo, no es que va a variar entre -75 y +75dps constantemente, sino que va a tomat un valor, digamos 15dps, y variará un poco alrededor de dicho valor.
Es muy importante determinar el offset de nuestro sensor para obtener una medicion correcta.
Sensivilidad (Sensitivity): Nos indica cuantos dps equivale cada bit del dato binario de salida. Por ejemplo, para un rango de 2000dps tenemos 0.07 dps/LSB. Esto significa que cuando la salida del sensor sea 1 representará 0.07 dps, cuando la salida sea 10 significará que el sensor está midiendo 0.7 dps. La sensibilidad es bastante fija y está muy cerca de lo que dice la hoja de datos. Se puede determinar prácticamente para mejorar la presicion de la medicion.
Rango(range): El rango es entre que valores el sensor puede medir. Por ejemplo, este sensor podee 3 rangos configurables: +-250dps, +-500dps y +-2000dps.

El offset y la sensivilidad son funcion de la temperatura y en aplicaciones de alta presicion se podría corregir con las variaciones de temperatura.

Otro dato importante es el ODR (Output Data Rate). Este sensor podemos elegir 100Hz (1 muestra cada 10mseg), 200Hz, 400Hz y 800Hz (1 medicion cada 1,25mseg). si no he leido mál a mayor ODR mayor nivel de ruido tendremos en la medicion.

Bien, con esto estamos mas o menos en condiciones de empezar a probar estos sensores. Para ello hay que tener un poco de idea de como funciona el bus I2C para entender mejor.

Edito: Un lugar con una excelente explicacion sobre la teoría de los Acelerómetros y gyróscopos.

« Última modificación: 20 de Enero de 2014, 21:27:09 por elgarbe »
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Giroscopos - UAV's
« Respuesta #1 en: 05 de Enero de 2014, 11:46:57 »
I2C Muy básico

Informacion sobre I2C hay por todos lados, yo simplemente haré una reseña de lo que necesita saber alguien que no nunca leyó nada al respecto y quiere hacer andar estos sensores.

El bus I2C es un bus de 2 cables, SCL (Clock) y SDA (Datos). Generalmente hay un maestro que inicia las comunicaciones y varios esclavos con direcciones únicas. El bus es bidireccional, por lo que el maestro puede pedir datos al esclavo y el esclavo responde con ellos. Cada dispositivo posee una direccion de 7bits. Se agrega un bit más que indica si la accion a realizar es una lectura o una escritura. No voy a entrar en detalles con el tema de bit de start, bit de stop, ACK y demás yerbas, eso para mí esta oculto en el drive del I2C.
Cada driver (I2C) presentará una interfaz determinada que nos permitirá realizar una comunicacion en el bus. Por ejemplo, mi drive para el LPC1769 (Cortex M3) tiene las siguientes funciones:
Código: [Seleccionar]
void I2C1_IRQHandler( void );
void I2C1Init( void );
uint32_t I2CStart( uint32_t portNum );
uint32_t I2CStop( uint32_t portNum );
uint32_t I2CEngine( uint32_t portNum );

En mi caso la cosa es muy simple, tengo una funcion denominada I2C1Init, la cual inicializa y configura el bus I2C 1 de mi uC. La velocidad y demás parámetros estan en unos defines.
Despues tengo un I2C_IRQHandler, en donde estpa el corazón del drive, ya que la comunicacion en mi uC se realiza enteramente con interrupciones.
Luego tengo I2CStart e I2CStop, los cuales nunca usé
Finalmente tengo la más importante I2CEngine. Los drive para los PIC generalmente tienn funciones como i2c_read, i2cwrite, etc. La funcion I2CEngine que tiene mi drive se encarga de hacer la combinacion completa desde el Start al Stop, haciendo los read y write necesarios para obtener una comunicacion completa.
Entonces en mi codigo del giróscopo tengo una funcion que me permite escribir registos en él:
Código: [Seleccionar]
void L3G4200D_write(unsigned char reg_address, unsigned char value){

I2CWriteLength[PORT_USED] = 3;
I2CReadLength[PORT_USED] = 0;
I2CMasterBuffer[PORT_USED][0] = L3G4200D_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = reg_address;
I2CMasterBuffer[PORT_USED][2] = value;

I2CEngine( PORT_USED );
}
Si bien la el uso de I2CEngine no es muy intuitivo, una vez que le he agarrado la mano va como piña.
En este caso la secuencia sería, el master genera el CLK y pone la señal de START. Escribe la direccion del dispositivo con los 7bits + el bit de lectura/escritura. Si no me equivoco el master espera un ACK del escalvo. Luego el master escribe la direccion del registro (reg_address) y el valor que desea escribir y espera por el ACK del esclavo. Entonces con esta función se hace una transaccion completa I2C.

Tambien tengo una funcion para leer registros del giróscopo:
Código: [Seleccionar]
unsigned char L3G4200D_read(unsigned char reg){
// uint32_t result=0;
I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 1;
I2CMasterBuffer[PORT_USED][0] = L3G4200D_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = reg;
I2CMasterBuffer[PORT_USED][2] = L3G4200D_READ_ADDR;

I2CEngine( PORT_USED );

return(I2CSlaveBuffer[PORT_USED][0]);
}
Como ven, para leer un registro, primero hay que hacer una operacion de escritura y pasar la direccion que queremos leer. Luego sigue una operacion de lectura. Aquí, I2CEngine hace todo de una.

Tambien tengo una funcion que me permite inicializar (configurar) el Giróscopo:
Código: [Seleccionar]
void L3G4200D_init(){

// L3G4200D_write(L3G4200D_CTRL_REG1, 0b00001111); // 100Hz, 12.5 CO, all axis enable
// L3G4200D_write(L3G4200D_CTRL_REG4, 0x20); // 0x00 250 dps, 0x10 500 dps, 0x20 2000dps

L3G4200D_write(L3G4200D_CTRL_REG2, 0b00011001); //
L3G4200D_write(L3G4200D_CTRL_REG3, 0x00); //
L3G4200D_write(L3G4200D_CTRL_REG4, 0b10110000); // 0x00 250 dps, 0x10 500 dps, 0x20 2000dps
L3G4200D_write(L3G4200D_CTRL_REG5, 0b00010011); //
L3G4200D_write(L3G4200D_CTRL_REG1, 0x0F); // 100Hz, 12.5 CO, all axis enable
}

Y finalmente la funcion que permite leer los datos "crudos" (RAW data) del gyroscopo:
Código: [Seleccionar]
void L3G4200D_read_data(){

// uint32_t result=0;
I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 6;
I2CMasterBuffer[PORT_USED][0] = L3G4200D_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = (L3G4200D_DATA_ADDR | (1<<7));
I2CMasterBuffer[PORT_USED][2] = L3G4200D_READ_ADDR;

res_g=I2CEngine( PORT_USED );

gyro_X = make_word2(I2CSlaveBuffer[PORT_USED][1], I2CSlaveBuffer[PORT_USED][0]);
gyro_Y = make_word2(I2CSlaveBuffer[PORT_USED][3], I2CSlaveBuffer[PORT_USED][2]);
gyro_Z = make_word2(I2CSlaveBuffer[PORT_USED][5], I2CSlaveBuffer[PORT_USED][4]);
}
En esta funcion es muy importante el tipo de datos de gyro_X, Y y Z. Deben ser de 16bits con signo. Cada compilador posee un tipo de datos para ellos, en mi caso es signed short. Tambien es importante notar que hay que leer 2 direcciones para formar el dato de cada eje.

Para la configuracion inicial del gyro, lo podemos discutir y probar más adelante. Hay muchas opciones, algunas de las cuales no entiendo bien. En principio lo más importante a configurar es el rango (con él queda definida la sensibilidad), el ODR y el power mode.

Bueno, con esto tenemos una base para arrancar con las pruebas. Es importante que si alguien quiere hacer pruebas vea como funciona su drive para el bus I2C.

Bueno, en la próxima, Leyendo el registro who_am_I.

Saludos!
« Última modificación: 10 de Enero de 2014, 23:49:11 por elgarbe »
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Giroscopos - UAV's
« Respuesta #2 en: 10 de Enero de 2014, 22:21:07 »
Leyendo el registro who_am_I

Bueno, en este punto necesitaremos la hoja de datos para ver un poco las características de nuestro giro y su configuracion.
En mi caso uso el L3G4200D de ST.
Lo primero que hay que analizar es la tabla 4 (mecánical characteristics)



Como primer parámetro tenemos el Rango del gyro, podemos elegir entre +-250, +-500 y +-2000. A mayor rango, vamos a poder medir velocidades angulares mayores y además será menos sensible a velocidades más pequeñas. En mi caso elegiré 2000dps.
Una vez elegido el rango, la sensivilidad queda determinada. En mi caso 0.07 dps por cada bit en el dato de salida.
Luego tenemos el ODR, podemos elegir 100, 200, 400 u 800Hz. O sea, 1 muestra cada 10mseg hasta 1 muestra cada 1,25mseg. Aquí hay algo para investigar que aún no he hecho. Tengo entendido que a mayor ODR mas "ruidosa" será la señal de salida. En principio elegiría 100 o 200Hz.
El resto de los parámetros los podemos ver un poco más adelante.

Bien, en la Tabla18 tenemos un listado de todos los registros del gyro, los cuales debemos/podemos leer/escribir para hacerlo funcionar.
El primer registro que podemos utilizar para testeo es el Who_am_I en la direccion 0x0F cuyo valor por defecto es 211 decimal.
Luego tenemos 5 registros de control, los cuales debemos configurar para obtener la respuesta deseada del gyro
Luego tenemos los 6 registros con los datos crudos de cada eje.

Bueno, veamos un pequeño ejemplo para leer el registro Who_am_I y verificar el funcionamiento de la comunicacion:

Lamentablemente para muchos, mi placa de desarrollo es la LPCXpresso, por lo que el código que propongo es para ella. Es C, por lo que se podrá modificar facilmente a cualquier otro uC.

Bueno, este es mi archivo L3G4200D.h
Código: [Seleccionar]
#ifndef L3G4200D_H_
#define L3G4200D_H_

#include "i2c.h"

extern volatile uint8_t I2CMasterBuffer[I2C_PORT_NUM][BUFSIZE];
extern volatile uint8_t I2CSlaveBuffer[I2C_PORT_NUM][BUFSIZE];
extern volatile uint32_t I2CReadLength[I2C_PORT_NUM];
extern volatile uint32_t I2CWriteLength[I2C_PORT_NUM];

#define L3G4200D_READ_ADDR      0xD3
#define L3G4200D_WRITE_ADDR     0xD2
#define L3G4200D_DATA_ADDR 0x28

#define L3G4200D_WHO_AM_I       0x0F

#define L3G4200D_CTRL_REG1     0x20
#define L3G4200D_CTRL_REG2     0x21
#define L3G4200D_CTRL_REG3     0x22
#define L3G4200D_CTRL_REG4     0x23
#define L3G4200D_CTRL_REG5     0x24
#define L3G4200D_REFERENCE     0x25
#define L3G4200D_OUT_TEMP      0x26
#define L3G4200D_STATUS_REG    0x27

#define L3G4200D_OUT_X_L       0x28
#define L3G4200D_OUT_X_H       0x29
#define L3G4200D_OUT_Y_L       0x2A
#define L3G4200D_OUT_Y_H       0x2B
#define L3G4200D_OUT_Z_L       0x2C
#define L3G4200D_OUT_Z_H       0x2D

#define L3G4200D_FIFO_CTRL_REG 0x2E
#define L3G4200D_FIFO_SRC_REG  0x2F

#define L3G4200D_INT1_CFG      0x30
#define L3G4200D_INT1_SRC      0x31
#define L3G4200D_INT1_THS_XH   0x32
#define L3G4200D_INT1_THS_XL   0x33
#define L3G4200D_INT1_THS_YH   0x34
#define L3G4200D_INT1_THS_YL   0x35
#define L3G4200D_INT1_THS_ZH   0x36
#define L3G4200D_INT1_THS_ZL   0x37
#define L3G4200D_INT1_DURATION 0x38

#define PORT_USED 1

void L3G4200D_init();
unsigned char L3G4200D_read(unsigned char reg);
unsigned char L3G4200D_write(unsigned char reg_address, unsigned char value);

void L3G4200D_init(){

L3G4200D_write(L3G4200D_CTRL_REG1, 0b00001111); // 100Hz, 12.5 CO, all axis enable
L3G4200D_write(L3G4200D_CTRL_REG4, 0x20); // 0x00 250 dps, 0x10 500 dps, 0x20 2000dps
}

unsigned char L3G4200D_read(unsigned char reg){
I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 1;
I2CMasterBuffer[PORT_USED][0] = L3G4200D_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = reg;
I2CMasterBuffer[PORT_USED][2] = L3G4200D_READ_ADDR;

I2CEngine( PORT_USED );

return(I2CSlaveBuffer[PORT_USED][0]);
}

unsigned char L3G4200D_write(unsigned char reg_address, unsigned char value){
I2CWriteLength[PORT_USED] = 3;
I2CReadLength[PORT_USED] = 0;
I2CMasterBuffer[PORT_USED][0] = L3G4200D_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = reg_address;
I2CMasterBuffer[PORT_USED][2] = value;

return(I2CEngine( PORT_USED ));
}

#endif /* L3G4200D_H_ */

Sé que en los .h van las definiciones y tendria que tener un L3G4200D.c, pero todavia me falta aprender algunas cosas sobre programar con varios .c, en especial con el alcance de las variables, etc.
Bueno, como ven al principio estan todos los define con las direcciones de cada registro. Luego tengo las funciones típicas necesarias para usar el gyro en esta aplicacion.

Mi main.c es este:
Código: [Seleccionar]
#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif

#include <cr_section_macros.h>
#include <NXP/crp.h>
#include "uart2.h"
#include <stdio.h>
#include "i2c.h"
#include "L3G4200D.h"


/*******************************************************************************
**   Main Function  main()
*******************************************************************************/
int main (void){
unsigned char who_am_I;

I2C1Init(); /* initialize I2c1 */
L3G4200D_init();
UART2_Init(56700); // Inicializo el UART a 56700

UART2_PrintString ("\r\nGiroscopo L3G4200D\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");

who_am_I=L3G4200D_read(L3G4200D_WHO_AM_I);

UART2_PrintString("\r\nEl registro who am I  contiene: ");
uart2_printUint32(who_am_I, 10);

while ( 1 ){
}
}

Simplemente se inicializa el I2C, luego se inicializa el gyro. Para ello solo hace falta escribir 2 registros. el REG1 configuramos 100Hz de ODR, 12.5 Hz de BW, habilitamos los tres ejes y ponemos el modo de energia en Normal. En el REG4 elegimos 2000dps

Finalmente inicializamos la UART, escribimos un mensaje para luego leer el registro who_am_I y mostrar su valor.

El terminal obtengo la siguiente salida
« Última modificación: 10 de Enero de 2014, 23:50:06 por elgarbe »
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Giroscopos - UAV's
« Respuesta #3 en: 12 de Enero de 2014, 13:08:23 »
Leyendo los datos RAW

Bueno, ahora que ya estamos "hablando" con el gyro, denemos poder leer la informacion de los dps que está midiendo el gyro.

Para ello simplemente voy a agregar una funcion que me permita leer los 6 registros de datos y unir X_L xon X_H para obtener el dato completo del eje X (al igual que con los otros ejes).

Código: [Seleccionar]
short make_word(unsigned char HB, unsigned char LB){
   return ((HB << 8) | LB);
}

void L3G4200D_read_data(){

uint32_t result=0;
I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 6;
I2CMasterBuffer[PORT_USED][0] = L3G4200D_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = (L3G4200D_DATA_ADDR | (1<<7));
I2CMasterBuffer[PORT_USED][2] = L3G4200D_READ_ADDR;

result=I2CEngine( PORT_USED );

gyro_X = make_word(I2CSlaveBuffer[PORT_USED][1], I2CSlaveBuffer[PORT_USED][0]);
gyro_Y = make_word(I2CSlaveBuffer[PORT_USED][3], I2CSlaveBuffer[PORT_USED][2]);
gyro_Z = make_word(I2CSlaveBuffer[PORT_USED][5], I2CSlaveBuffer[PORT_USED][4]);
}

Como se puede ver se leen 6 registros, a partir de la direccion L3G4200D_DATA_ADDR. Luego se arma el dato combinando de a dos los registros leídos para armar un short con signo (16 bits).

En el main tenemos:
Código: [Seleccionar]
/*
===============================================================================
 Name        : main.c
 Author      : $(author)
 Version     :
 Copyright   : $(copyright)
 Description : main definition
===============================================================================
*/

#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif

signed short gyro_X, gyro_Y, gyro_Z;
volatile uint32_t msTicks;

__INLINE static void delay_ms (uint32_t delayTicks) {
  uint32_t currentTicks;
  currentTicks = msTicks; // read current tick counter
  while ((msTicks - currentTicks) < delayTicks);
}

#include <cr_section_macros.h>
#include <NXP/crp.h>
#include "uart2.h"
#include <stdio.h>
#include "i2c.h"
#include "L3G4200D.h"

float x1,y1,z1;

void SysTick_Handler(void) {
msTicks++; /* increment counter necessary in Delay() */
}

/*******************************************************************************
**   Main Function  main()
*******************************************************************************/
int main (void){

//Configuro el SysTick para que interrumpa cada 1mseg
if (SysTick_Config(SystemCoreClock / 1000)) {
while (1);
}

I2C1Init(); // Inicializamos el bus I2C
L3G4200D_init(); // Inicializamos el Gyróscopo
UART2_Init(57600); // Inicializamos el UART a 57600

UART2_PrintString ("\r\nGiroscopo L3G4200D\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");

UART2_PrintString ("\r\nRAW X\tRAW Y\tRAW Z\r\n");

while ( 1 ){

L3G4200D_read_data();

uart2_printInt32(gyro_X, 10);
UART2_Sendchar('\t');
uart2_printInt32(gyro_Y, 10);
UART2_Sendchar('\t');
uart2_printInt32(gyro_Z, 10);
UART2_Sendchar('\r');
UART2_Sendchar('\n');

delay_ms(500);
}
}

Como se puede ver, tenemos 3 variables globales signed short para almacenar los datos del gyro.
Aquí estamos haciendo uso del SysTick timer de los Cortex M3. Está configurado para que interrumpa cada 1 mseg, lo usamos para crear la funcion delay_ms.
Como antes, inicializamos el bus I2C, el gyróscopo y la UART. Luego llamamos a la funcion read_data del L3G4200D e imprimimos en la UART el resultado.

Esta es una imagen de nuestra terminal:



Bien, esos son los datos crudos del gyro, no son dps. La primera observacion que podemos hacer es que no son 0 como uno esperaría obtener con el gyro sin mover, fijo en la mesa... En cambio obtenemos ciertos valores. Bueno, esos valores son el Zero Rate Level. Otra cosa que observamos es que dicho valor cambia constantemente. Por ejemplo en el eje X tenemos valores que van desde 7 a 18 "cuentas del ADC". Esto puede ser mucho o poco, aún no lo sabemos ya que no tenemos los datos en dps para compararlos con los 2000dps de alcance con el que tenemos configurado al gyro. Aunque si pensamos un poco, el gyro tiene un ADC de 16 y me parece que obtener una variacion de 7 a 18 cuentas en 65535 no parece ser mucho, me parece que es simplemente "ruido" en el ADC... pero de esto no estoy seguro. Algo que no entiendo, ya que nunca estudie, es el Rate Noise Density... quizá tenga que ver con esto... alguien esta leyendo este post y sabe al respecto puede desasnarme...

Bien, entonces el siguiente paso lógico sería convertir los datos RAW en dps.

Eso quedará para la próxima entrega.
Saludos!
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Giroscopos - UAV's
« Respuesta #4 en: 12 de Enero de 2014, 14:39:59 »
Eliminando Offset - obteniendo °/seg (dps)

Bueno, llegó el momento de que este bichito empiece a dar informacion que sirva para algo!

Como vimos en el post anterior, el gyróscopo no entrega 0 cuentas cuando está en reposo, sino que posee un Zero Rate Level. Es por ello que debemos realizar una funcion que obtenga el offset del Gyro para poder restarlo a cada medicion posterior y obtener una lectura correcta.
Mi funcion de obtener offset es la siguiente:

Código: [Seleccionar]
void L3G4200D_GetBiass(void){

int i;
for (i = 0; i < SAMPLESS_BIASS; i += 1) {
L3G4200D_read_data();
biass_X += gyro_X;
biass_Y += gyro_Y;
biass_Z += gyro_Z;
delay_ms(1);
}
biass_X /= SAMPLESS_BIASS;
biass_Y /= SAMPLESS_BIASS;
biass_Z /= SAMPLESS_BIASS;
}

Como pueden observar, simplemente tomo SAMPLESS_BIASS cantidad de muestras y hago un promedio de ellas. En este caso estoy tomando unas 600 muestras. Cuantas más muestras mejor.

Una vez que obtengo el Offset, lo siguiente es la conversion de LSB's o cuentas o datos RAW en dps. Para ello utilizamos la sensibilidad. En la tabla 4 antes vista, encontramos que para 2000dps de rango tenemos una sensibilidad de 70 mdps/LSB o 0.07 dps/LSB.
En definitiva los datos "correctos" o reales son (RAW - Offset) * Sensibilidad

Bien, mi main queda ahora de la siguiente forma:

Código: [Seleccionar]
/*
===============================================================================
 Name        : main.c
 Author      : $(author)
 Version     :
 Copyright   : $(copyright)
 Description : main definition
===============================================================================
*/

#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif

#define GYRO_X_SCALE 0.07 //Sensibilidad en cada eje
#define GYRO_Y_SCALE 0.07 //extraído del datasheet
#define GYRO_Z_SCALE 0.07

#define SAMPLESS_BIASS 600 //Número de muestras para calular biass

signed short gyro_X, gyro_Y, gyro_Z;
double biass_X, biass_Y, biass_Z;

volatile uint32_t msTicks;

__INLINE static void delay_ms (uint32_t delayTicks) {
  uint32_t currentTicks;
  currentTicks = msTicks; // read current tick counter
  while ((msTicks - currentTicks) < delayTicks);
}

#include <cr_section_macros.h>
#include <NXP/crp.h>
#include <math.h>
#include "uart2.h"
#include <stdio.h>
#include "i2c.h"
#include "L3G4200D.h"

void SysTick_Handler(void) {
msTicks++; /* increment counter necessary in Delay() */
}

/*******************************************************************************
**   Main Function  main()
*******************************************************************************/
int main (void){

double gx, gy, gz;

//Configuro el SysTick para que interrumpa cada 1mseg
if (SysTick_Config(SystemCoreClock / 1000)) {
while (1);
}

I2C1Init(); // Inicializamos el bus I2C
L3G4200D_init(); // Inicializamos el Gyróscopo
UART2_Init(57600); // Inicializamos el UART a 57600

UART2_PrintString ("\r\nGiroscopo L3G4200D\r\n");
UART2_PrintString ("======== elgarbe =========\r\n");

UART2_PrintString ("Obteniendo Offset...\r\n");

L3G4200D_GetBiass();

UART2_PrintString ("Offset X: ");
uart2_printDouble(biass_X, 4);
UART2_PrintString("\r\n");
UART2_PrintString ("Offset Y: ");
uart2_printDouble(biass_Y, 4);
UART2_PrintString("\r\n");
UART2_PrintString ("Offset Z: ");
uart2_printDouble(biass_Z, 4);
UART2_PrintString("\r\n");

UART2_PrintString ("\r\nRAW X\tRAW Y\tRAW Z\tdps X\tdps Y\tdps Z\r\n");

while ( 1 ){

L3G4200D_read_data();

gx=((float)gyro_X - biass_X);
gy=((float)gyro_Y - biass_Y);
gz=((float)gyro_Z - biass_Z);

uart2_printDouble(gx , 2);
UART2_Sendchar('\t');
uart2_printDouble(gy , 2);
UART2_Sendchar('\t');
uart2_printDouble(gz , 2);
UART2_Sendchar('\t');

gx *= GYRO_X_SCALE;
gy *= GYRO_Y_SCALE;
gz *= GYRO_Z_SCALE;

uart2_printDouble(gx, 3);
UART2_Sendchar('\t');
uart2_printDouble(gy, 3);
UART2_Sendchar('\t');
uart2_printDouble(gz, 3);
UART2_PrintString("\r\n");

delay_ms(500);
}
}

Bueno, básicamente, primero obtenemos el offset con la nueva funcion incorporada. Muestro esos datos.
Luego calculo los datos sin offset y los muestro. Finalmente multiplico por la sensibilidad y muestro los datos en °/seg
Esta es una captura de lo que obtengo:



De aquí podemos ver aunque hemos eliminado el offset, todavía tenemos datos distintos de cero, pero en este caso están mas cerca de 0. Tamien notamos que en el peor de los casos tenemos 0.564 dps en el eje X cuando deveríamos tener 0. Será mucho? Pues veamos, el rango del Gyro está en 2000 dps por lo que 0.564 representa el 0.028%!!!! A mí me parece que eso es más que suficiente!

Bien, veamos ahora que pasa si giramos el sensor en un sentido y lo dejamos quieto, pero inclinado:



Como se puede observar he girado el sensor "alrededor" del eje X y lo he hecho a una velocidad aproximada de 30°/seg. Cuando dejé de girar el sensor, el resultado vuelve a ser 0 ya que estoy midiendo la velocidad angular.

Bueno, ya estamos llegando al final, solo nos queda calcular el ángulo a partir de la velocidad...

Saludos!
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Giroscopos - UAV's
« Respuesta #5 en: 15 de Enero de 2014, 21:24:23 »
Integrando y obteniendo el ángulo de giro

Bien, ya estamos listos para poder obtener el tan preciado ángulo que ha girado la IMU. Para ello debemos integrar la velocidad angular que nos brinda el Gyróscopo en un tiempo de muestreo determinado.
La técnica es bien simple, tomo un tiempo de muestreo (20mseg en mi caso) y leo el sensor cada timeStep segundos. La integracion discreta se obtiene tomando el valor anterior del ángulo + el valor nuevo * el timeStep.

Entonces mi nuevo main es:

Código: [Seleccionar]
#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif

#define GYRO_X_SCALE 0.07 //Sensibilidad en cada eje
#define GYRO_Y_SCALE 0.07 //extraído del datasheet
#define GYRO_Z_SCALE 0.07

#define SAMPLESS_BIASS 600 //Número de muestras para calular biass

signed short gyro_X, gyro_Y, gyro_Z;
double biass_X, biass_Y, biass_Z;

volatile uint32_t msTicks;

__INLINE static void delay_ms (uint32_t delayTicks) {
  uint32_t currentTicks;
  currentTicks = msTicks; // read current tick counter
  while ((msTicks - currentTicks) < delayTicks);
}

#include <cr_section_macros.h>
#include <NXP/crp.h>
#include <math.h>
#include "uart2.h"
#include <stdio.h>
#include "i2c.h"
#include "L3G4200D.h"

int timeStep = 20; //Tiempo que tiene que durar el main


void SysTick_Handler(void) {
msTicks++; /* increment counter necessary in Delay() */
}

/*******************************************************************************
**   Main Function  main()
*******************************************************************************/
int main (void){
double gx=0.0, gy=0.0, gz=0.0;
uint32_t timer=0;
//Configuro el SysTick para que interrumpa cada 1useg
if (SysTick_Config(SystemCoreClock / 1000)) {
while (1);
}

I2C1Init(); // Inicializamos el bus I2C
L3G4200D_init(); // Inicializamos el Gyróscopo
UART2_Init(57600); // Inicializamos el UART a 57600

UART2_PrintString ("\r\nGiroscopo L3G4200D\r\n");
UART2_PrintString ("======== elgarbe =========\r\n");

UART2_PrintString ("Obteniendo Offset...\r\n");

L3G4200D_GetBiass();

UART2_PrintString ("Offset X: ");
uart2_printDouble(biass_X, 4);
UART2_PrintString("\r\n");
UART2_PrintString ("Offset Y: ");
uart2_printDouble(biass_Y, 4);
UART2_PrintString("\r\n");
UART2_PrintString ("Offset Z: ");
uart2_printDouble(biass_Z, 4);
UART2_PrintString("\r\n");

UART2_PrintString ("\r\n° X\t° Y\t° Z\r\n");


timer = msTicks; //get a start value to determine the time the loop takes

while ( 1 ){
L3G4200D_read_data();

gx += (((float)gyro_X - biass_X) * GYRO_X_SCALE) * timeStep/1000;
gy += (((float)gyro_Y - biass_Y) * GYRO_Y_SCALE) * timeStep/1000;
gz += (((float)gyro_Z - biass_Z) * GYRO_Z_SCALE) * timeStep/1000;

uart2_printDouble(gx, 3);
UART2_Sendchar(',');
uart2_printDouble(gy, 3);
UART2_Sendchar(',');
uart2_printDouble(gz, 3);
// UART2_Sendchar(',');
// uart2_printInt32(msTicks, 10);
UART2_PrintString("\r\n");

timer=msTicks-timer; // Obtengo cuánto tiempo pasó en el bucle
timer=timeStep-timer; // Obtengo lo que falta para llegar al timeStep
delay_ms(timer); // Espero hasta llegar al timeStep
timer=msTicks;
}
}

Como podemos ver simplemente hemos agregado la integracion discreta y al final del loop tenemos un par de instrucciones para calcular cuanto tiempo debo "perder" en mi bucle para llegar a los timeStep segundos.

Bueno, en el hyperterminal he logueado en un archivo los datos leidos del sensor para luego abrirlo con Excel y realizar el siguiente gráfico:



Como se puede observar, en la prueba he girado el sensor alrededor del eje X, primero 90° en un sentido, luego lo vuelvo a la posicion original luego lo giro en otro sentido y finalmente lo dejo quieto unos segundos.
El tiempo del eje horizontal es en segundos.
De este gráfico podemos ver 2 cosas cuando giro el sensor en un sentido hasta obtener máxima excursion, el ángulo no llega a 90°. Algirar en el otro sentido se pasa de 90°.
Finalmente podemos ver el tan odiado DRIFT. Cuando dejo el sensor quieto, debido al proceso de integracion, los datos no estan fijos, sino que el ángulo va creciendo en un sentido, es por ellos que los gyróscopos no suelen usarse solos para determinar la posicion del objeto que hemos rotado.
Para corregir el drift se utilizan los acelerómetros. Veremos su funcionamiento en el post dedicado a ellos.

Saludos!
-
Leonardo Garberoglio

Desconectado PCCM

  • PIC16
  • ***
  • Mensajes: 109
Re: Giroscopos - UAV's
« Respuesta #6 en: 15 de Enero de 2014, 23:39:34 »
Tengo entendido que el "Rate Noise Density"(Rn) como bien dices es el ruido del sensor.
Generalmente se denota el efecto del ruido por este parámetro o por el "Angular Random Walk".
La formula del Ruido del sensor está dada por: Ruido Sensor=(Rate Noise Density)*sqrt(Ancho de banda)
A más ancho de banda, más ruido se verá en el sensor.

Tal Rn se puede representar como la fluctuación de la medida del sensor. Para el caso de este sensor el  ruido del sensor será 0.03*sqrt(50)=0.212 dps, con la condición de que el ancho de banda sea de 50Hz como dice en el diagrama.

hay otros parámetros que también afectan al ruido total, pero para hallar el "ruido total"(RT) es:
RT=sqrt(Rn + .....), con este ejemplo y considerando solo el Rn, el RT dá aprox  0.46 dps.

Como dije otro parámetro común de encontrar en los gyros u otros sensores es en "Angular Random Walk"
Para convertir el "Rate Noise Density" a "Angular Random Walk"(ARW) solo se multiplica por 60.
Por ejemplo en este caso el Rn=0.03dps/sqrt(Hz)
entonces el ARW=0.03*60=1.8 grados/sqrt(hora), el ARW describe la desviación promedio cuando se integra la señal. Independientemente de otras características que contribuyen al error angular(como el factor escala o las bias).
El error incrementará a lo largo de la integración, por ejemplo para este caso dentro de una hora la desviación angular promedio será de 1.8*sqrt(1)=1.8 grados, después de 2 horas será 1.8*sqrt(2)= 2.54 grados.



Desconectado PCCM

  • PIC16
  • ***
  • Mensajes: 109
Re: Giroscopos - UAV's
« Respuesta #7 en: 16 de Enero de 2014, 00:01:22 »
Te resultaron esos valores debido al tiempo de integración que le colocaste(20 ms), el cual es demasiado como para calcular el ángulo con el giroscopo, si bajas la velocidad de muestreo solo para este ejemplo mejorará su comportamiento.

La velocidad de muestreo se puede aumentar a esos valores de 20ms o alrededor, cuando se tiene un algoritmo para corregir el angulo.
además recuerda corregir el factor escala.

Saludos
« Última modificación: 16 de Enero de 2014, 00:07:50 por PCCM »

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Giroscopos - UAV's
« Respuesta #8 en: 16 de Enero de 2014, 00:12:01 »
Totalmente de acuerdo. Bajando el tiempo del lazo (tiempo de integracion) o mejorando el algoritmo de integracion se puede reducir y mucho la deriva.
La idea es mostrar que dicha deriva existe y que generalmente no se utiliza el giróscopo sin corregir con otro sensor debido a ello.
Puede ser que el corrimiento del cero se deba a que el sensor no esta perfectamente a 0° cuando está en reposo? deduzco que debe estar inlcinado unos 10° en el sentido del eje Y. por eso obtengo 20° de menos en un sentido y 20° de más en el otro.
Tambien tendría que trabajar en el factor de escala, por ahora y para estos ejemplos tomo el de la hoja de datos. Creo que lo ideal para el factor de escala sería hacerlo girar a velocidad constante y verificar con la lectura.
Todas esa mejoras y calibraciones harán que la deriva se reduzca, pero no desaparecerá....

Por cierto, como vienes con tu proyecto?

Saludos!
-
Leonardo Garberoglio

Desconectado PCCM

  • PIC16
  • ***
  • Mensajes: 109
Re: Giroscopos - UAV's
« Respuesta #9 en: 16 de Enero de 2014, 00:24:50 »
Ya que tu micro puede correr más rápido puedes mostrar los resultados a su máximo muestreo y ver como se comporta. yo no pude llegar ya que con toda la transmisión serial llegaba a 8ms.

a mediados de la prox. semana retomo el navegador inercial en el trabajo, ya que aún tengo problemas. Ahora estoy acabando otro proyecto, que me consume todo el tiempo.
Gracias por seguir compartiendo. Así animas a profundizase más en el tema.

saludos.

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Giroscopos - UAV's
« Respuesta #10 en: 16 de Enero de 2014, 09:25:45 »
Tengo entendido que el "Rate Noise Density"(Rn) como bien dices es el ruido del sensor.
Generalmente se denota el efecto del ruido por este parámetro o por el "Angular Random Walk".
La formula del Ruido del sensor está dada por: Ruido Sensor=(Rate Noise Density)*sqrt(Ancho de banda)
A más ancho de banda, más ruido se verá en el sensor.

Tal Rn se puede representar como la fluctuación de la medida del sensor. Para el caso de este sensor el  ruido del sensor será 0.03*sqrt(50)=0.212 dps, con la condición de que el ancho de banda sea de 50Hz como dice en el diagrama.

hay otros parámetros que también afectan al ruido total, pero para hallar el "ruido total"(RT) es:
RT=sqrt(Rn + .....), con este ejemplo y considerando solo el Rn, el RT dá aprox  0.46 dps.

Como dije otro parámetro común de encontrar en los gyros u otros sensores es en "Angular Random Walk"
Para convertir el "Rate Noise Density" a "Angular Random Walk"(ARW) solo se multiplica por 60.
Por ejemplo en este caso el Rn=0.03dps/sqrt(Hz)
entonces el ARW=0.03*60=1.8 grados/sqrt(hora), el ARW describe la desviación promedio cuando se integra la señal. Independientemente de otras características que contribuyen al error angular(como el factor escala o las bias).
El error incrementará a lo largo de la integración, por ejemplo para este caso dentro de una hora la desviación angular promedio será de 1.8*sqrt(1)=1.8 grados, después de 2 horas será 1.8*sqrt(2)= 2.54 grados.


Fantàstico, ahora entiendo un poco más el tema!
En este contexto, que sería el ancho de banda? por que es 50Hz en tu ejemplo? eso es otra cosa que siempre tuve como duda...

Saludos!
-
Leonardo Garberoglio

Desconectado PCCM

  • PIC16
  • ***
  • Mensajes: 109
Re: Giroscopos - UAV's
« Respuesta #11 en: 17 de Enero de 2014, 03:45:39 »
Le puse 50Hz ya que en la imagen que colocaste, en el "Test condition" del "Rate Noise density", dice que calcularon ese valor con esa condición.
El valor de 0.03 dps/sqrt(Hz) es un promedio del análisis del "Rn" en los 3 ejes y en todos los rangos.
Por ejemplo en un ejemplo de STMicroeletronics en la siguiente tabla se ve como obtuvieron el "Rn" para un gyro, realizandole 6 test, en todos los rangos con un ancho de banda de 12.5Hz.


El promedio de todos estos "Rn" es 0.029dps/sqrt(Hz) el cual es dato para el datasheet del sensor.

El ancho de banda es el rango de frecuencia en la cual el sensor va a obtener los datos, cuanto más ancho de banda el sensor será más sensible y por la formula anterior tendrá mas ruido, "sqrt" veces el ancho de banda, generalmente se define este valor para el filtro pasa bajos que incorpora los gyros digitales.Un mejor ejemplo está en la siguiente imagen del mpu6050.


Para el caso de gyro se puede observar que se puede configurar varios anchos de banda desde 5Hz a 256Hz, para el filtro pasa bajos. Estos filtros tienen su demora de procesamiento como se describe en la parte derecha. y los cuales estos datos pueden ser muestreados optimamente a la frecuencia especificada más a la derecha.

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Giroscopos - UAV's
« Respuesta #12 en: 17 de Enero de 2014, 23:30:08 »
Ya que tu micro puede correr más rápido puedes mostrar los resultados a su máximo muestreo y ver como se comporta. yo no pude llegar ya que con toda la transmisión serial llegaba a 8ms.

a mediados de la prox. semana retomo el navegador inercial en el trabajo, ya que aún tengo problemas. Ahora estoy acabando otro proyecto, que me consume todo el tiempo.
Gracias por seguir compartiendo. Así animas a profundizase más en el tema.

saludos.

Bueno, siguiendo tu consejo he hecho la prueba. Mi tiempo de muestreo está en 5mseg. Para que te des una idea, la lectura del giro toma 846uSeg, el envío de los datos a 115200bps toma 1535uSeg.

El resultado es mucho mejor:



Notar que el experimento fue por 96seg. y ver lo planchado que esta al comienzo y al final...
Para tener una idea, en el segundo 62.082 es cuando el gyro quedó estable:
2,714   4,808   3,029   62,080
en el segundo 96.8 tengo:
4,085   4,999   3   96,800

son 1.3° para el eje X, 0.19° para el Y y 0.029 para el Z!!!!!!!!!!!!!!!!!!!!!!

Respecto al 1.3° del eje X te digo que otra cosa que noté es la variacion del Offset del gyro. Si obtengo 10 veces el offset obtengo valores distintos, similares, pero distintos. He visto proyectos en donde para sacar el offset toman 1000 muestras y obtienen 10 veces el offset y toman el promedio de las 10.

También probe, pero no documenté, una especie de filtro con la desviacion estándar. Lo que hacía es es tomar 600 mediciones y promediarlar. Luego tomaba 600 muestras y calculaba la desviacion estándar. Mi "filtro" era cualquier cosa por debajo de la desviacion estándar era considerado 0....

Tengo que reconocer que de ruido y sus filtros en sensores conozco poco y nada...

En fin, esa deriba que estoy obteniendo para mí es más que suficiente, pensar que no está corregida con ningun otro sensor... Tambien hay que tener en cuenta que 5mseg en el bucle principal es medio complicado, cuando se agrega el mantenimiento de la DCM y la lectura de los otros sensores no sé si se podrá llegar a ese tiempo... En breve espero poder probarlo y documentarlo...

Ahora lo que tendría que probar es pasar el I2C a 400KHz y tratar de aumentar el ODR para ver si se pueden achicar los tiempos...

Saludos!
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Giroscopos - UAV's
« Respuesta #13 en: 18 de Enero de 2014, 00:01:02 »
Bueno, una más antes de irme a dormir....

Probé el cálculo del Offset varias veces para obtener el promedio, esto es lo que obtuve:



Los datos son estos:

Código: [Seleccionar]
Giroscopo L3G4200D
======== elgarbe =========
Obteniendo Offset,,,

Off X Off Y Off Z
14,7650 -6,6450 -16,9767
13,6813 -6,2011 -16,2850
16,4128 -6,9820 -16,1655
14,1807 -6,1783 -15,8136
12,0853 -6,1503 -15,4730
11,4201 -6,4936 -15,4841
14,6824 -7,7158 -17,5491
14,1978 -6,6879 -16,6009
11,2137 -6,3128 -15,8093
13,4737 -6,7789 -15,9997
14,6541 -7,7446 -17,3417
15,6844 -5,6879 -16,4706
15,7995 -6,9811 -17,2991
12,5280 -6,6216 -16,2238
15,0509 -7,4894 -17,0370
12,1918 -7,4025 -15,7367
10,7087 -5,5807 -15,0629
14,6795 -6,5976 -17,2384
17,5895 -6,3977 -17,2421
14,3876 -6,6740 -17,2337
17,1590 -6,1561 -17,3587
12,9019 -8,3619 -17,2173
16,6715 -5,5073 -16,5554
11,5845 -4,4708 -14,5943
14,3793 -5,1458 -14,9893
14,3073 -5,5852 -15,9983
15,4022 -5,8226 -15,3983
15,7257 -5,6230 -16,9540
17,4695 -6,1977 -17,3816
12,4408 -6,7937 -17,0023
15,8257 -5,3447 -16,0317
15,0364 -7,8256 -18,6051
15,1984 -5,7264 -16,8127
15,7703 -7,0829 -17,5664
12,4696 -6,8835 -16,2193
14,5774 -6,3431 -16,2870
13,7460 -6,8506 -17,4171
16,0412 -7,1298 -17,6457
13,0134 -7,0202 -17,0961
15,7667 -6,5450 -17,1052
15,2813 -6,0992 -17,0935
14,8471 -7,7768 -18,2035
12,7764 -6,0563 -16,0737
15,6930 -5,7568 -16,3551
15,7678 -6,5696 -16,6073
14,8113 -7,0726 -17,9110
13,0630 -8,0501 -17,9765
14,6934 -4,5884 -15,0300
12,2912 -6,6126 -16,9900
14,7655 -6,7760 -17,1867

14,3773 -6,5019 -16,6541

La última fila es el promedio de los 50 offset's.... en el gráfico puse una linea de tendencia Lineal con su ecuacion....
Conclusiones?

Saludos!
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Giroscopos - UAV's
« Respuesta #14 en: 18 de Enero de 2014, 15:52:40 »
Bueno, un experimento más.

Calculo el offset promediando 50 juegos de offset, cada uno de 600 muestras.

El resultado:



Luego tomo el promedio de esos 50 juegos de offset y ejecuto el algoritmo que integra la velocidad angular para obtener el ángulo en el que se encuentra el sensro y obtengo:




Como resultado se destaca:

En el segundo 97 tengo:
2,425   6,706   2,24   97

y al final, en el segundo 158 tengo:

4,408   6,913   1,369   158

Graficamente:



En el eje X tengo algo raro, es como si fuese muy ruidoso y es el que más deriva tiene. Luego el eje Y tiene 0.2° en 1 minuto. El eje Z sufrió un pequeño codazo mío que hizo que el se gire un poco, por eso el salto en el valor. Pero en el gráfico se ve que es muy estable....

Alguna idea que pasará con el eje X???

Saludos!
-
Leonardo Garberoglio


 

anything