Obteniendo Gauss - Verificando calibracionBueno, llegó el momento de que este bichito empiece a dar informacion que sirva para algo!
Como ya hemos comentado y como bien nos ha mostrado jonathan, el magnetómetro mide el campo magnético de la tierra. En mi caso estoy usando la medicion con los eje X e Y paralelos a la tierra. Por lo tanto, con ellos realizaré la medicion.
Así como los gyro y accel tenían probelmas de zero rate level y había que hacer una calibracion más o menos complicada, los magnetómetro tienen lo suyo.
El campo magnético de la tierra es muy débil y por lo tanto suceptible a muchas perturbaciones. un imán cercano, una mesa metálica, un motor eléctrico funcionando cerca, son todas fuentes de perturbaciones. Por lo tanto es de esperarce que para un correcto funcionamiento se deba calibrar el magnetómetro en el dispositivo final en el que será usado.
Bueno, veamos el código.
Mi HMC5883L.h es el siguiente:
#ifndef HMC5883L_H_
#define HMC5883L_H_
#include "i2c.h"
#include <math.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 HMC5883L_READ_ADDR 0x3D
#define HMC5883L_WRITE_ADDR 0x3C
#define Config_Reg_A 0x00
#define Config_Reg_B 0x01
#define Mode_Reg 0x02
#define X_MSB_Reg 0x03
#define X_LSB_Reg 0x04
#define Z_MSB_Reg 0x05
#define Z_LSB_Reg 0x06
#define Y_MSB_Reg 0x07
#define Y_LSB_Reg 0x08
#define Status_Reg 0x09
#define ID_Reg_A 0x0A
#define ID_Reg_B 0x0B
#define ID_Reg_C 0x0C
#define PORT_USED 1
short make_word(unsigned char HB, unsigned char LB);
void HMC5883L_init();
char HMC5883L_read(unsigned char reg);
char HMC5883L_write(unsigned char reg_address, unsigned char value);
char HMC5883L_read_data();
short make_word(unsigned char HB, unsigned char LB){
return ((HB << 8) | LB);
}
void HMC5883L_init(){
HMC5883L_write(Config_Reg_A, 0b00010000); //Configuramos el magnetómetro sin promedio de muestras
HMC5883L_write(Config_Reg_B, 0b00100000); //15Hz de ODR, Normal mode, 1.3Ga de rango
HMC5883L_write(Mode_Reg, 0x00); //Modo de sensado continuo
}
char HMC5883L_read(unsigned char reg){
I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 1;
I2CMasterBuffer[PORT_USED][0] = HMC5883L_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = reg;
I2CMasterBuffer[PORT_USED][2] = HMC5883L_READ_ADDR;
I2CEngine( PORT_USED );
return(I2CSlaveBuffer[PORT_USED][0]);
}
char HMC5883L_write(unsigned char reg_address, unsigned char value){
I2CWriteLength[PORT_USED] = 3;
I2CReadLength[PORT_USED] = 0;
I2CMasterBuffer[PORT_USED][0] = HMC5883L_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = reg_address;
I2CMasterBuffer[PORT_USED][2] = value;
return(I2CEngine( PORT_USED ));
}
char HMC5883L_read_data(){
uint32_t result=0;
I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 6;
I2CMasterBuffer[PORT_USED][0] = HMC5883L_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = X_MSB_Reg;
I2CMasterBuffer[PORT_USED][2] = HMC5883L_READ_ADDR;
result=I2CEngine( PORT_USED );
mag_X = make_word(I2CSlaveBuffer[PORT_USED][0], I2CSlaveBuffer[PORT_USED][1]);
mag_Z = make_word(I2CSlaveBuffer[PORT_USED][2], I2CSlaveBuffer[PORT_USED][3]);
mag_Y = make_word(I2CSlaveBuffer[PORT_USED][4], I2CSlaveBuffer[PORT_USED][5]);
return(result);
}
#endif /* HMC5883L_H_ */
Como se puede ver en la inicializacion solo configuramos el sensor para que haga lecturas continuas, sin promediarlas, un ODR de 15Hz y un rango de +-1.3Gauss. Con este rango, la sensibilidad queda determinada en 0.92mgauss/LSB.
En esta aplicacion lo que estoy buscando es mostrar gráficamente los datos que devuelve el magnetómetro con y sin perturvaciones. Es por ello que no haeré aún el cálculo del ángulo respecto del Norte.
Mi main.c es el siguiente:
#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif
#include <cr_section_macros.h>
#include <NXP/crp.h>
signed short mag_X = 0;
signed short mag_Y = 0;
signed short mag_Z = 0;
volatile uint32_t msTicks; /* counts 1ms timeTicks */
// ****************
// systick_delay - creates a delay of the appropriate number of Systicks (happens every 1 ms)
__INLINE static void delay_ms (uint32_t delayTicks) {
uint32_t currentTicks;
currentTicks = msTicks; // read current tick counter
// Now loop until required number of ticks passes.
while ((msTicks - currentTicks) < delayTicks);
}
#include "i2c.h"
#include "HMC5883l.h"
#include "uart2.h"
void SysTick_Handler(void) {
msTicks++; /* increment counter necessary in Delay() */
}
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void)
{
float gaus_X=0.0;
float gaus_Y=0.0;
float gaus_Z=0.0;
//Configuro el SysTick para que interrumpa cada 1mseg
if (SysTick_Config(SystemCoreClock / 1000)) {
while (1); // Capture error
}
I2C1Init(); // Inicializamos el bus I2C
HMC5883L_init(); // Inicializamos el Magnetómetro
UART2_Init(115200); // Inicializamos el UART a 115200
UART2_PrintString ("\rMagnetómetro HMC5883L\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");
UART2_PrintString ("\r\nGauss X\tGauss Y\tGauss Z\r\n");
while ( 1 ){
HMC5883L_read_data();
gaus_X = (float)mag_X * 0.92/1000;
gaus_Y = (float)mag_Y * 0.92/1000;
gaus_Z = (float)mag_Z * 0.92/1000;
uart2_printDouble(gaus_X, 4);
UART2_Sendchar('\t');
uart2_printDouble(gaus_Y, 4);
UART2_Sendchar('\t');
uart2_printDouble(gaus_Z, 4);
UART2_Sendchar('\r');
UART2_Sendchar('\n');
delay_ms(500);
}
}
Como se puede observar simplemente leemos los datos RAW y los multiplicamos por la sensibilidad. La divicion por 1000 es para obtener el resultado en gauss. Según la wikipedia el
campo magnético de la tierra ronda los 0.25 a 0.65 gauss.
Veamos que obtenemos nosotros:
como se puede ver estamos en concordancia con lo esperado.
Bueno, ahora biene lo importante, antes de poder leer el ángulo de desviacion respecto al Norte magnético (o al norte geográfico si hacemos la correccion que explicó jonathan en 2 post más arriba) debemos asegurarnos que el offset y la sensibilidad de cada eje sea correcto. Para ello vamos a loguear datos cada xx mseg del eje X y del eje Y. Luego haremos un gráfico X vs Y en excel para ver la figura que se forma. La teoría dice que debería ser un círculo centrado en el origen... veamos que obtengo:
Bueno, como puede observarse tenemos un círculo casi perfecto, el tema es que lejos está de estar centrado en el origen.
Bueno, empecemos a estudiar los datos. Lo primero que debemos obtener es el máximo y el mínimo en cada eje, en mi caso obtengo:
Maximo X 0,3551 Minimo X 0,011
Maximo Y -0,1592 Minimo Y -0,495
con estos datos podemos obtener el rango en cada eje:
Rango X 0,3441
Rango Y 0,3358
Como podemos ver son muy parecidos, por lo que no parece requerir ajuste de sensivilidad. Incluso la pequeña diferencia puede deberse a alguna inclinacion involuntaria del sensor mientras lo giraba...
Bueno, con estos datos calculo el offset con el que hay que ajustar cada eje para obtener el círculo centrado en el origen:
Offset X 0,18305
Offset Y -0,3271
restando dicho valor a cada lectura del magnetómetro obtendremos la lectura corregida.
Veamos ahora el resultado aplicando ese offset a los datos previamente logueados:
Bueno, ahora sí! un círculo casi perfecto!!!!
Este algoritmo de calibración debería implementarse en el uC y ejecutarlo al montarlo en el dispositivo final. Esos valores de offset deberían ser definitivos.
Pueden ver en el primer post la llamada calibracion para hard iron y para soft iron.
Bueno, con el magnetómetro corregido podemos obtener el tan deseado ángulo respecto al norte. Para ello utilizaremos el trabajo de Jonathan y sus librerías.
Saludos!