Autor Tema: Acelerómetros - UAV's  (Leído 9563 veces)

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

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Acelerómetros - UAV's
« en: 05 de Enero de 2014, 11:48:53 »
Los acelerómetros son sensores que miden la aceleracion a la que el sensor es sometido. Si bien existen sensores que miden en uno o dos ejes, hoy en día son muy comunes los acelerómetros que miden en los tres ejes.
Debido a que todo cuerpo en la superficie terrestre está sometido a la "Fuerza de la gravedad", el sensor en estado de reposo medirá la "aceleracion de la gravedad". Las aceleraciones pueden compararce contra dicha aceleración y por lo tanto 9,8m/seg2 se toma como 1g. Entonces nuestro acelerómetro expresará su salida en tantos g's.
En mi caso estoy trabajando y mostraré resultados aquí del ADXL345 de Analog Devices. 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 acelerómetro esta "quieto" debería entregar en su salida 0 g (1g en el eje Z). Sin embargo no es así y con el acelerómetro en reposo siempre tendremos un valor distinto de 0. En la hoja de datos del ADXL, en la Tabla 1, podemos ver que el Zero g OFFSET es del orden de los +-150mg en los ejes X e Y y +-250mg en el eje Z.
Es muy importante determinar el offset de nuestro sensor para obtener una medicion correcta.
Sensivilidad (Sensitivity): Nos indica cuantos mg equivale cada bit del dato binario de salida. Por ejemplo, para un rango de +-16g tenemos 31.2 mg/LSB. Esto significa que cuando la salida del sensor sea 1 representará 0.0312 g, cuando la salida sea 10 significará que el sensor está midiendo 0.312 g. 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 4 rangos configurables: +-16g, +-8g y +-4g y +-2g.

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 entre 0.1Hz hasta 3200Hz. 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. Ver acá.

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

Saludos
« Última modificación: 20 de Enero de 2014, 21:26:56 por elgarbe »
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Acelerómetros - UAV's
« Respuesta #1 en: 05 de Enero de 2014, 11:54:22 »
Leyendo el registro Device Id

Bueno, en este punto necesitaremos la hoja de datos para ver un poco las características de nuestro Acelerómetro y su configuracion.
En mi caso uso el ADXL345 de Analog.
Lo primero que hay que analizar es la tabla 1



Como primer parámetro tenemos el Rango del Accel, podemos elegir entre +-2, +-4, +-8 y +-16. A mayor rango, vamos a poder medir aceleraciones mayores y además será menos sensible a aceleraciones más pequeñas. Este sensor posee una foma de trabajar que permite matener la mejor sensibilidad (3.9mg) usando cualquier rango. Esto se activa eligiendo Full Resolution. En mi caso elegiré +-16g con full resolution activado.
Una vez elegido el rango, la sensivilidad queda determinada. En mi caso 3.9mg por cada bit en el dato de salida.
Luego tenemos el ODR, podemos elegir entre un amplio rango, desde 0.1 hasta 3200Hz. 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 Tabla19 tenemos un listado de todos los registros del accel, los cuales debemos/podemos leer/escribir para hacerlo funcionar.
El primer registro que podemos utilizar para testeo es el Device Id (como el Who_am_I del gyro) en la direccion 0x00 cuyo valor por defecto es 229 decimal.
Tenemos varios registros de control, los cuales debemos configurar para obtener la respuesta deseada del acelerómetro. Los más importantes son BW_RATE, POWER_CTL y DATA_FORMAT
Tenemos tambien los 6 registros con los datos crudos de cada eje a partir de la direccion 0x32

Bueno, veamos un pequeño ejemplo para leer el registro Dev_Id 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 ADXL345.h
Código: [Seleccionar]
/*
 * ADXL345.h
 *
 *  Created on: 04/12/2013
 *      Author: elgarbe
 */

#ifndef ADXL345_H_
#define ADXL345_H_

#define ADXL345_READ_ADDR       0xA7
#define ADXL345_WRITE_ADDR      0xA6

#define ADXL345_DEV_ID       0x00
#define ADXL345_POWER_CTL 0x2D
#define ADXL345_BW_RATE 0x2C
#define ADXL345_DATA_FORMAT 0x31
#define ADXL345_DATA_ADDR 0x32

#define PORT_USED 1

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];

short make_word(unsigned char HB, unsigned char LB);
void ADXL345_init();
unsigned char ADXL345_read(unsigned char reg);
unsigned char ADXL345_write(unsigned char reg_address, unsigned char value);
unsigned char ADXL345_read_data();

short make_word(unsigned char HB, unsigned char LB){
   return ((HB << 8) | LB);
}

void ADXL345_init(){

ADXL345_write(ADXL345_POWER_CTL, 0b00000000);

ADXL345_write(ADXL345_DATA_FORMAT, 0b00001011); // 1XXX Full res, XX11 +-2g

ADXL345_write(ADXL345_BW_RATE, 0b00001010); // 100Hz Data Output Rate

ADXL345_write(ADXL345_POWER_CTL, 0b00001000);

}


unsigned char ADXL345_read(unsigned char reg){

I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 1;
I2CMasterBuffer[PORT_USED][0] = ADXL345_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = reg;
I2CMasterBuffer[PORT_USED][2] = ADXL345_READ_ADDR;

I2CEngine( PORT_USED );

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


unsigned char ADXL345_write(unsigned char reg_address, unsigned char value){

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

return(I2CEngine( PORT_USED ));
}
#endif /* ADXL345_H_ */

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 accel en esta aplicacion.

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

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

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

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

I2C1Init(); /* initialize I2c1 */
ADXL345_init();
UART2_Init(115200); // Inicializo el UART a 115200

UART2_PrintString ("\rAcelerómetro ADXL345\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");

dev_id=ADXL345_read(ADXL345_DEV_ID);

UART2_PrintString("\r\nEl registro DEVICE ID  contiene: ");
uart2_printUint32(dev_id, 10);

while ( 1 ){
}
}

Simplemente se inicializa el I2C, luego se inicializa el accel. Para ello solo hace falta escribir 3 registros. El POWER_CTL con 0x00 para asegurar el modo SLEEP previo a la configuracion del sensor, DATA_FORMAT configuramos Full Resolution y +-16g, BW_RATE en 100Hz

Finalmente inicializamos la UART, escribimos un mensaje y luego leo el registro Dev_Id y mostrar su valor.

El terminal obtengo la siguiente salida


Bueno, la próxima lectura de los datos crudos.

Saludos!
« Última modificación: 20 de Enero de 2014, 21:04:50 por elgarbe »
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Acelerómetros - UAV's
« Respuesta #2 en: 20 de Enero de 2014, 21:22:14 »
Leyendo los datos RAW

Bueno, ahora que ya estamos "hablando" con el accel, debemos poder leer la informacion de los G que está midiendo el acelerómetro.

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);
}

unsigned char ADXL345_read_data(){

char result;
I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 6;
I2CMasterBuffer[PORT_USED][0] = ADXL345_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = ADXL345_DATA_ADDR;
I2CMasterBuffer[PORT_USED][2] = ADXL345_READ_ADDR;

result=I2CEngine( PORT_USED );

accel_X = make_word(I2CSlaveBuffer[PORT_USED][1], I2CSlaveBuffer[PORT_USED][0]);
accel_Y = make_word(I2CSlaveBuffer[PORT_USED][3], I2CSlaveBuffer[PORT_USED][2]);
accel_Z = make_word(I2CSlaveBuffer[PORT_USED][5], I2CSlaveBuffer[PORT_USED][4]);

return(result);
}

Como se puede ver se leen 6 registros, a partir de la direccion ADXL345_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 accel_X, accel_Y, accel_Z;

volatile uint32_t msTicks; /* counts 1ms timeTicks */

__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 "i2c.h"
#include "ADXL345.h"
#include "uart2.h"


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
ADXL345_init(); // Inicializamos el Gyróscopo
UART2_Init(115200); // Inicializamos el UART a 115200

UART2_PrintString ("\rAcelerómetro ADXL345\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");

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

while ( 1 ){

ADXL345_read_data();

uart2_printInt32(accel_X, 10);
UART2_Sendchar('\t');
uart2_printInt32(accel_Y, 10);
UART2_Sendchar('\t');
uart2_printInt32(accel_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 accel.
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 acelerómetro y la UART. Luego llamamos a la funcion read_data del ADXL345 e imprimimos en la UART el resultado.

Esta es una imagen de nuestra terminal:



Bien, esos son los datos crudos del gyro, no son G (aceleracion respecto a la gravedad). La primera observacion que podemos hacer es que no son 0 como uno esperaría obtener con el acelerómetro sin mover, fijo en la mesa (para el eje X e Y, el eje Z si debe medir algo, debe medir la aceleracion de la gravedad)... En cambio obtenemos ciertos valores. Bueno, esos valores son el Zero g Level. Otra cosa que observamos es que dicho valor cambia constantemente. Por ejemplo en el eje X tenemos valores que van de 0 a -1 "cuentas del ADC". Esto puede ser mucho o poco, aún no lo sabemos ya que no tenemos los datos en G para compararlos con los 16g de alcance con el que tenemos configurado al accel. Aunque si pensamos un poco, el accel tiene un ADC de 13bits y me parece que obtener una variacion de 1 cuentas en 65535 no parece ser mucho, me parece que es simplemente "ruido" en el ADC... pero de esto no estoy seguro.
Bien, entonces el siguiente paso lógico sería convertir los datos RAW en G.

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

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Acelerómetros - UAV's
« Respuesta #3 en: 20 de Enero de 2014, 22:16:32 »
Eliminando Offset - obteniendo G's

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

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

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

int i;
for (i = 0; i < SAMPLESS_BIASS; i += 1) {
ADXL345_read_data();
biass_X += accel_X;
biass_Y += accel_Y;
biass_Z += accel_Z;
delay_ms(1);
}
biass_X /= SAMPLESS_BIASS;
biass_Y /= SAMPLESS_BIASS;
biass_Z /= SAMPLESS_BIASS;
biass_Z -= 256;
}

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. Notar que en el eje X no debemos restar lo que corresponde a 1G (1/0.0039 = 256).

Una vez que obtengo el Offset, lo siguiente es la conversion de LSB's o cuentas o datos RAW en G's. Para ello utilizamos la sensibilidad. En la tabla 1 antes vista, encontramos que en full resolution tenemos una sensibilidad de 3.9 mg/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 ACEL_X_SCALE 0.0039 //Sensibilidad en cada eje
#define ACEL_Y_SCALE 0.0039 //extraído del datasheet
#define ACEL_Z_SCALE 0.0039

signed short accel_X, accel_Y, accel_Z;
float x1,y1,z1;
double biass_X, biass_Y, biass_Z;

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

volatile uint32_t msTicks; /* counts 1ms timeTicks */

__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 "i2c.h"
#include "ADXL345.h"
#include "uart2.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
ADXL345_init(); // Inicializamos el Gyróscopo
UART2_Init(115200); // Inicializamos el UART a 115200

UART2_PrintString ("\rAcelerómetro ADXL345\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");

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

ADXL345_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\tGs X\tGs Y\tGs Z\r\n");

while ( 1 ){

ADXL345_read_data();

gx=((float)accel_X - biass_X);
gy=((float)accel_Y - biass_Y);
gz=((float)accel_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 *= ACEL_X_SCALE;
gy *= ACEL_Y_SCALE;
gz *= ACEL_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 G's
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.014 G's en el eje Y cuando deveríamos tener 0. Será mucho? Pues veamos, el rango del Accel está en 16G por lo que 0.014 representa el 0.087%!!!! A mí me parece que eso es más que suficiente! Tambien notemos que es un poco mas "ruidos" que el gyro en el que teníamos 0.028%

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 hasta un ángulo de 45°. Cuando dejé de girar el sensor, el resultado es que tengo medicion en el eje Y y en el eje Z. Esto es porque al girar el sensor empieza a medir la aceleracion de la gravedad en alguno de los ejes.

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

Saludos!
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Acelerómetros - UAV's
« Respuesta #4 en: 20 de Enero de 2014, 22:51:46 »
Calculando el ángulo de giro

Bien, ya estamos listos para poder obtener el tan preciado ángulo que ha girado la IMU con el acelerómetro. Para ello debemos simplemente usar funciones trigonométrias.
La técnica es bien simple, .

Entonces mi nuevo main es:

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

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

#define ACEL_X_SCALE 0.0039 //Sensibilidad en cada eje
#define ACEL_Y_SCALE 0.0039 //extraído del datasheet
#define ACEL_Z_SCALE 0.0039
#define PI 3.1415926

signed short accel_X, accel_Y, accel_Z;
float x1,y1,z1;
double biass_X, biass_Y, biass_Z;

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

volatile uint32_t msTicks; /* counts 1ms timeTicks */

__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 "i2c.h"
#include "ADXL345.h"
#include "uart2.h"

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
ADXL345_init(); // Inicializamos el Gyróscopo
UART2_Init(115200); // Inicializamos el UART a 115200

UART2_PrintString ("\rAcelerómetro ADXL345\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");

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

ADXL345_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\ng's X\tg's Y\tg's Z\tpitch\troll\r\n");

while ( 1 ){
double pitchAccel, rollAccel;

ADXL345_read_data();

x1=((float)accel_X - biass_X) * ACEL_X_SCALE;
y1=((float)accel_Y - biass_Y) * ACEL_Y_SCALE;
z1=((float)accel_Z - biass_Z) * ACEL_Z_SCALE;

pitchAccel = atan2(y1, z1) * 360.0 / (2*PI);
rollAccel = -asin(x1) * 360.0 / (2*PI);

uart2_printDouble(x1, 3);
UART2_Sendchar('\t');
uart2_printDouble(y1, 3);
UART2_Sendchar('\t');
uart2_printDouble(z1, 3);
UART2_Sendchar('\t');
uart2_printDouble(pitchAccel, 3);
UART2_Sendchar('\t');
uart2_printDouble(rollAccel, 3);
UART2_Sendchar('\r');
UART2_Sendchar('\n');
delay_ms(20);
}
}

En este caso no hace falta el control del tiempo en el loop ya que no hay integracion, es simple cálculo trigonométrico

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 eje horizontal no tiene unidad, es simplemente el número de muestra
Como podemos ver este sensor no sufre de drift o deriva. Si lo dejo quieto mide siempre lo mismo, aunque pase el tiempo. Pero presenta un problema, es "ruidoso" ver en el segundo giro que aparecen unos escalones en la rampa de subida.
Como podemos concluir cada sensor tiene un pro y una contra. El acelerómetro no tiene deriva, pero detecta cualquier vibracion. El gyro es más estable en su medicion, menos inmune a las altas frecuencias, pero tiene deriva...

El paso siguiente será buscar una técnica (filtro) que nos permita mezclar los dos sensores y obtener una sola medicion del ángulo de giro...

Saludos!
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Acelerómetros - UAV's
« Respuesta #5 en: 21 de Enero de 2014, 23:17:43 »
Comparando medición de Acelerómetro y gyróscopo

Antes de comenzar con la comparación de las mediciones y la posterior fusión de las mismas, hay que recordar un par de cosas.
Lo primero es entender que el gyróscopo mide la velocidad angular del UAV y al integrarla obtenemos el ángulo de gyro. Hay que tener en cuenta que con el gyróscopo obtenemos el ángulo de giro con el Marco de Referencia del UAV.
El acelerómetro, en cambio, mide la inclinacion del UAV respecto de la Gravedad. La cual es fija en el Marco de Referencia de la Tierra. Por lo tanto las mediciones no son compatibles y deberemos realizar alguna pequeña transformación para poder obtener las mediciones en el mismo Marco de Referencia, generalmente, el de la Tierra.
Los giróscopos, al integrar la velocidad angular para obtener el ángulo, va presentando deriva. Es decir, que aunque el sensor esté quieto, se va integrando (acumulando) el ruido. Estos sensores son poco sensibles a las altas frecuencias, por lo que su salida suele ser bien suave aunque hagamos movimientos bruscos.
Los acelerómetros miden el ángulo de giro respecto al vector de la gravedad. El resultado es funcion de funciones senos y cosenos, las cuales tienen zonas más lineales/sensible que otras. Por ejemplo, el seno de ángulos bajos es más sensible al seno de angulos cercanos a 90°. Esto es, la diferencia entre el seno de 10° y el seno de 20° es 0,1579. En cambio la diferencia entre el seno de 70° y el de 80° es 0,045. Por otra parte los acelerómetros son muy sensibles a las altas frecuancias y cualquier vibracion se ve reflejado en la salida de los mismo.

Todo esto esta mejor explicado en este video de youtube:

Bien, si quisiéramos hacer un dispositivo que se oriente unicamente con acelerómetros podríamos poner un buen filtro pasa bajos para eliminar el ruido del acelerómetro. Si bien esto funcionaría, el filtro introducirá un retardo considerable entre el movimiento real y el apreciado a la salida del filtro.
Si quisieramos hacer un sistema que se oriente solo con gyróscopos tendriamos como problema la deriva. Aunque hay sistemas en donde no es tan importante la deriva, como por ejemplo los mouse con tecnología MEMs. Ya que lo que se mide es la orientacion relativa a la posicion anterior... o algo por el estilo...

Bien, podemos ver que cada sensor tiene un fuerte y una devilidad y aprovechando los fuertes de cada uno podemos construir un mejor sistema de orientacion. A esto se le llama fusion de los datos de los sensores. Algoritmos de fuión hay muchos, entre ellos un simple filtro complementario, el filtro de kalman, el de kalman extendido para sistemas no lineales, Madwgik, etc.

Bien, pero antes de fuisionar los datos debemos poder ver las características de cada sensor. Para ello he realizado un nuevo programa en el cual se loguean ambos sensores para luego mostrarlo en Excel.

Mi main.c es el siguiente:

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 ACEL_X_SCALE 0.0039 //Sensibilidad en cada eje
#define ACEL_Y_SCALE 0.0039 //extraído del datasheet
#define ACEL_Z_SCALE 0.0039

#define SAMPLESS_BIASS 1000 //Número de muestras para calular biass
#define PI 3.1415926

volatile uint32_t usTicks;
float rollGyro=0.0, pitchGyro=0.0;
float pitchAccel=0.0, rollAccel=0.0;
int timeStep = 10000; //Tiempo que tiene que durar el main en uSegfloat x1,y1,z1;
#define sampTime  timeStep/1000000 //SampleTime en segundos
float xg, yg, zg, xa, ya, za;

signed short accel_X, accel_Y, accel_Z;
float biasAccelX, biasAccelY, biasAccelZ;
signed short gyro_X, gyro_Y, gyro_Z;
float biasGyroX, biasGyroY, biasGyroZ;

__INLINE static void delay_us (uint32_t delayTicks) {
  uint32_t currentTicks;
  currentTicks = usTicks; // read current tick counter
  while ((usTicks - 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"
#include "ADXL345.h"

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


/*******************************************************************************
**   Main Function  main()
*******************************************************************************/
int main (void){
uint32_t timer=0;

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

I2C1Init(); /* initialize I2c1 */
ADXL345_init(); // Inicializamos el Acelerometro
L3G4200D_init(); // Inicializamos el Gyróscopo
UART2_Init(115200); // Inicializo el UART a 115200

L3G4200D_GetBiass(); // Obtengo el offset del giróscopo
ADXL345_GetBiass(); // Obtengo el offset del acelerometro

UART2_PrintString ("\r\nAcelerometro ADXL345 y Giroscopo L3G4200D\r\n");
UART2_PrintString ("\r\nPitch A\tRoll A\tPitch G\tRoll G\r\n");

timer = usTicks; //get a start value to determine the time the loop takes
while ( 1 ){
L3G4200D_read_data();
xg=((float)gyro_X - biasGyroX) * GYRO_X_SCALE;
yg=((float)gyro_Y - biasGyroY) * GYRO_Y_SCALE;
zg=((float)gyro_Z - biasGyroZ) * GYRO_Z_SCALE;
pitchGyro = pitchGyro + xg * sampTime;
rollGyro = rollGyro - yg * sampTime;

ADXL345_read_data();
xa=((float)accel_X - biasAccelX) * ACEL_X_SCALE;
ya=((float)accel_Y - biasAccelY) * ACEL_Y_SCALE;
za=((float)accel_Z - biasAccelZ) * ACEL_Z_SCALE;
//Calculate the pitch in degrees as measured by the accelerometers.
pitchAccel = atan2(ya, za) * 360.0 / (2*PI);
rollAccel = -asin(xa) * 360.0 / (2*PI);
uart2_printDouble(rollAccel, 3);
UART2_Sendchar('\t');
uart2_printDouble(rollGyro, 3);
UART2_Sendchar('\t');
uart2_printDouble(pitchAccel, 3);
UART2_Sendchar('\t');
uart2_printDouble(pitchGyro, 3);
UART2_Sendchar('\r');
UART2_Sendchar('\n');

timer=usTicks-timer; // Obtengo cuánto tiempo pasó en el bucle
timer=timeStep-timer; // Obtengo lo que falta para llegar al timeStep
delay_us(timer); // Espero hasta llegar al timeStep
timer=usTicks;                    //make one loop last time step msec
}
}

Como se puede observar, primero defino la sensibilidad de cada sensor de acuerdo al datasheet. Defino la cantidad de muestras para la obtención del offset de cada sensor y algunas variables necesarias.
Mi funcion de delay ahora está en uSeg para poder tener mejor control del tiempo del bucle. En este caso estoy trabajando a 10mSeg.

Luego, como siempre, inicializo el I2C, la UART, el Gyro y el Accel. Obtengo el offset de cada uno y lo imprimo en el terminal.
En el bucle principal calculo el ángulo con el Gyro primero y luego con el acelerómetro. Luego logueo los dos. Finalmente hago control de tiempo de loop.

El resultado obtenido es el siguiente:



Bien, aquí podemos ver el "ruido" del acelerómetro en verde y celeste y la deriva final del giróscopo en rojo y violeta.

El paso siguiente será fusionar los datos para obtener una respuesta mas suabe y sin deriva...

Saludos!
-
Leonardo Garberoglio

Desconectado rivale

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 1707
Re: Acelerómetros - UAV's
« Respuesta #6 en: 22 de Enero de 2014, 11:32:02 »
Muy buen trabajo Elgarbe, he leido tus post y estan muy bien explicados, creo que faltaria que hicieras referencia a tus otros post.

Yo he hecho uso de acelerómetro y de giroscopios pero para un robot equilibrista, yo utlicé el filtro kalman para unir ambos datos, pero me gustaría saber mas sobre los otros filtros que piensas usar

Saludos
"Nada es imposible, no si puedes imaginarlo"

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Acelerómetros - UAV's
« Respuesta #7 en: 22 de Enero de 2014, 11:38:12 »
Muy buen trabajo Elgarbe, he leido tus post y estan muy bien explicados, creo que faltaria que hicieras referencia a tus otros post.

Yo he hecho uso de acelerómetro y de giroscopios pero para un robot equilibrista, yo utlicé el filtro kalman para unir ambos datos, pero me gustaría saber mas sobre los otros filtros que piensas usar

Saludos

Hola! me alegro que sea util!
Ok, voy a armar una especie de índice así queda todo más ordenado.
Muy interesante lo del robot equilibrista! Lo tenes funcionando? Yo andaba con ganas de intentar armar uno de esos vehículos de 2 ruedas que aceleran cuando lo inclinas hacia adelante y se mantienen vericales. Pero por ahora tengo que ir cerrando el tema de los sensores y la controladora de vuelo.

El tema filtros estoy empezando a investigar. Por ahora voy a arrancar mostrando el complementario, que como dijo Pablo es una sola línea y es simple de implementar. El kalman lo implementaste estudiando el sistema dinámico en donde está aplicado para determinar las matrices? Ahora estoy por abrir un post sobre ese tema la fusion de sensores con filtros y si queres me contás lo que hiciste así me ahorras investigacion!!!!

Saludos y gracias!
-
Leonardo Garberoglio

Desconectado rivale

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 1707
Re: Acelerómetros - UAV's
« Respuesta #8 en: 22 de Enero de 2014, 11:57:45 »
Muy interesante lo del robot equilibrista! Lo tenes funcionando? Yo andaba con ganas de intentar armar uno de esos vehículos de 2 ruedas que aceleran cuando lo inclinas hacia adelante y se mantienen vericales. Pero por ahora tengo que ir cerrando el tema de los sensores y la controladora de vuelo.

El tema filtros estoy empezando a investigar. Por ahora voy a arrancar mostrando el complementario, que como dijo Pablo es una sola línea y es simple de implementar. El kalman lo implementaste estudiando el sistema dinámico en donde está aplicado para determinar las matrices? Ahora estoy por abrir un post sobre ese tema la fusion de sensores con filtros y si queres me contás lo que hiciste así me ahorras investigacion!!!!


mi robotito, o vehículo, porque esta grandote, esta funcionando, en cuanto a lo del filtro, aunque lei sobre la teoria del filtro y como armar la matriz, utilice variables que ya estaba probadas para los sensores que utilicé, solo hice algunos cambios a las constantes

en este post tratamos los robots equilibristas, ahi hay muchos links útiles sobre el filtro kalman y el complementario, creo que no hay sobre algún otro.
"Nada es imposible, no si puedes imaginarlo"

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Acelerómetros - UAV's
« Respuesta #9 en: 22 de Enero de 2014, 22:56:43 »
Una cosa que queda pendiente tanto con el giróscopo como con el acelerómetro es obtener la sensivilidad. Como ven en el gráfico anterior, al comparar el acelerómetro con el giróscopo no obtengo el mismo angulo, hay una pequeña diferencia, atribuible a pequeñas diferencias en la sensibilidad.
El que más me preocupa es el acelrómetro. He visto algoritmos de calibracion en el que se pide colocar el acelerómetro en distintas posiciones, se obtienen lecturas y luego se hacen cáluclos para obtener la sensivilidad. Tengo mis dudas de esos algoritmos ya que, creo, necesitan que se coloque el sensor con cada eje perfectamente vertical/horizontal y por lo tanto depende mucho de que el usuario coloque el sensor en la posicion esperada.
Yo pensaba hacer algo más o menos así:
Pedir al usuario que coloque el sensor con el eje Z mirando hacia abajo (mas o menos)
Empezar a tomar muestras y pedir al usuario que gire el acelerómetro lentamente alrededor del punto vertical mientras tomamos muestras.
Determinamos el máximo valor leido.
Pedimos al usuario que gire 180° el acelerómetro y ahora quede el eje Z mirando hacia arriba.
Hacemos lo mismo.
Lo que hemos hecho es obtener el valor RAW de 1g en un sentido y luego en el otro. Restando ambos valores y dividiendo por dos tenemos el valor raw de 1g. Tambien obtenemos de aquí el offset, ya que el valor maximo en un sentido puede ser mayor que en el otro sentido. Luego dividiendo por la cantidad de bits del ADC obtenemos la sensibilidad....

Que les parece? funcionaría? conocen otro algoritmo?

Saludos!
-
Leonardo Garberoglio

Desconectado PCCM

  • PIC16
  • ***
  • Mensajes: 109
Re: Acelerómetros - UAV's
« Respuesta #10 en: 23 de Enero de 2014, 01:20:36 »
Claro parece buena técnica, y sería para los 3 ejes, ya que como dices la calibración en la mesa del usuario no es perfectamente horizontal.
Yo uso una técnica que tiene una idea similar.

Me guié de este pdf, en la página 3("Correction of Sensor Bias Error and Sensitivity Error"):

http://www.kionix.com/sites/default/files/AN012%20Accelerometer%20Errors.pdf

Lo debes posicionar en 6 posiciones distintas y en cada una de ellas sensar el acelerómetro, como veras es bien parecida, solo que acá no hallan el valor máximo durante el giro sino sacan un promedio de las diversas posiciones colocadas.

La sensibilidad esta dada por Sxx,Syy,Szz y las bias por Bx, By, Bz( en gravedad 0)
La lectura calibrada del acel seria.

Acel_calibrado= (Acel_leido - B)/S


Desconectado rivale

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 1707
Re: Acelerómetros - UAV's
« Respuesta #11 en: 23 de Enero de 2014, 11:33:16 »
mira esto

http://www.ehowenespanol.com/encontrar-sensibilidad-del-acelerometro-como_292587/

es prácticamente lo mismoq ue planteas, creo que esto necesitarias hacerlo solo una vez para caracterizar tu sensor y después ya no seria necesario
"Nada es imposible, no si puedes imaginarlo"

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Acelerómetros - UAV's
« Respuesta #12 en: 23 de Enero de 2014, 12:46:09 »
mira esto

http://www.ehowenespanol.com/encontrar-sensibilidad-del-acelerometro-como_292587/

es prácticamente lo mismoq ue planteas, creo que esto necesitarias hacerlo solo una vez para caracterizar tu sensor y después ya no seria necesario

el problema que creo tiene ese metodo es que supones que la aceleración 1g en el primer paso, pero para eso el sensor tiene que tener el eje Z perfectamente vertical, cosa que es imposible de determinar. por eso yo sugería registrar y mover el acelerometro cerca de la vertical, buscando la mayor lectura. la mayor lectura sí puedo asegurar que es la medición correspondiente a 1g. me explico? o estoy cometiendo un error?..

Saludos!
-
Leonardo Garberoglio

Desconectado rivale

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 1707
Re: Acelerómetros - UAV's
« Respuesta #13 en: 23 de Enero de 2014, 12:56:57 »
ya entendi, buscas que el usuario lo lleve al extremo de la medición, en ambos sentidos, asi tienes +1g y -1g.

"Nada es imposible, no si puedes imaginarlo"

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Acelerómetros - UAV's
« Respuesta #14 en: 23 de Enero de 2014, 13:04:47 »
Claro parece buena técnica, y sería para los 3 ejes, ya que como dices la calibración en la mesa del usuario no es perfectamente horizontal.
Yo uso una técnica que tiene una idea similar.

Me guié de este pdf, en la página 3("Correction of Sensor Bias Error and Sensitivity Error"):

http://www.kionix.com/sites/default/files/AN012%20Accelerometer%20Errors.pdf

Lo debes posicionar en 6 posiciones distintas y en cada una de ellas sensar el acelerómetro, como veras es bien parecida, solo que acá no hallan el valor máximo durante el giro sino sacan un promedio de las diversas posiciones colocadas.

La sensibilidad esta dada por Sxx,Syy,Szz y las bias por Bx, By, Bz( en gravedad 0)
La lectura calibrada del acel seria.

Acel_calibrado= (Acel_leido - B)/S


mi duda es cuanto error se puede cometer al suponer que en la posicion 1 el eje z esta bien vertical. a lo mejor exagero un poco porque mi imu esta enchufada en un protoboard y se mueve para todos lados... quizás si el sensor esta en una placa junto con el uC y uno puede ver que esta bastante horizontal alcanza. al fin y al cabo todas las implementaciones que he visto son como la que muestras, poner el sensor en 6 posiciones distintas y medir.
gano algo aumentando el numero de posiciones? digo, si mido en 12 posiciones diferentes, obtendría mejores resultados?
en este link, starlino usa 12 posiciones y habla del metodo de los menores cuadrados? no he tenido tiempo de mirarlo bien....

Saludos!!
-
Leonardo Garberoglio


 

anything