/*
* HMC5883L.h
*
* Created on: 04/12/2013
* Author: elgarbe
*/
#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);
short make_word(unsigned char HB, unsigned char LB){
return ((HB << 8) | LB);
}
void HMC5883L_init(){
HMC5883L_write(Config_Reg_A, 0x70); //Configuramos el magnetómetro
HMC5883L_write(Config_Reg_B, 0xA0);
HMC5883L_write(Mode_Reg, 0x00);
}
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 ));
}
#endif /* HMC5883L_H_ */
#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif
#include <cr_section_macros.h>
#include <NXP/crp.h>
#include "i2c.h"
#include "HMC5883l.h"
#include "uart2.h"
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void){
unsigned char dev_id;
I2C1Init(); /* initialize I2c1 */
HMC5883L_init();
UART2_Init(115200); // Inicializo el UART a 115200
UART2_PrintString ("\rMagnetómetro HMC5883L\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");
dev_id=HMC5883L_read(ID_Reg_A);
UART2_PrintString("\r\nEl registro Identificatio Register A contiene: ");
uart2_printUint32(dev_id, 10);
dev_id=HMC5883L_read(ID_Reg_B);
UART2_PrintString("\r\nEl registro Identificatio Register B contiene: ");
uart2_printUint32(dev_id, 10);
dev_id=HMC5883L_read(ID_Reg_C);
UART2_PrintString("\r\nEl registro Identificatio Register C contiene: ");
uart2_printUint32(dev_id, 10);
while ( 1 ){
}
}
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 );
X_axis = make_word(I2CSlaveBuffer[PORT_USED][0], I2CSlaveBuffer[PORT_USED][1]);
Z_axis = make_word(I2CSlaveBuffer[PORT_USED][2], I2CSlaveBuffer[PORT_USED][3]);
Y_axis = make_word(I2CSlaveBuffer[PORT_USED][4], I2CSlaveBuffer[PORT_USED][5]);
return(result);
}
#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif
#include <cr_section_macros.h>
#include <NXP/crp.h>
signed short mag_X, mag_Y, mag_Z;
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)
{
//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\nRAW X\tRAW Y\tRAW Z\r\n");
while ( 1 ){
HMC5883L_read_data();
uart2_printInt32(mag_X, 10);
UART2_Sendchar('\t');
uart2_printInt32(mag_Y, 10);
UART2_Sendchar('\t');
uart2_printInt32(mag_Z, 10);
UART2_Sendchar('\r');
UART2_Sendchar('\n');
delay_ms(500);
}
}
#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_ */
#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);
}
}
#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;
#define PI 3.141592653589793238462643383279502884197f
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;
float heading=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\tHeading\r\n");
while ( 1 ){
HMC5883L_read_data();
gaus_X = (float)mag_X * 0.92/1000 - 0.18305; //Resto el offset obtenido en el Excel
gaus_Y = (float)mag_Y * 0.92/1000 + 0.3271;
gaus_Z = (float)mag_Z * 0.92/1000;
heading = atan2(gaus_Y, gaus_X); //Calculo el ángulo respecto al Norte Magnético
if(heading < 0.0){ //Si el valor es negativo
heading += (2.0 * PI); //sumamos 2*Pi para que no queden cortes en transiciones
} // de 179° a -180°
if(heading > (2.0 * PI)){ // Por si el valor es mayor a 2 PI
heading -= (2.0 * PI);
}
heading *= (180.0 / PI); //Paso a °
uart2_printDouble(gaus_X, 4);
UART2_Sendchar('\t');
uart2_printDouble(gaus_Y, 4);
UART2_Sendchar('\t');
uart2_printDouble(heading, 4);
UART2_Sendchar('\r');
UART2_Sendchar('\n');
delay_ms(500);
}
}
Magnetómetro HMC5883L
======== elgarbe ==========
Gauss X Gauss Y Heading
0.1629 0.0318 11.0411
0.1675 0.0088 3.0011
0.1629 -0.0170 354.0482
0.1583 -0.0418 345.1989
0.1564 -0.0455 343.7822
0.1518 -0.0602 338.3654
0.1417 -0.0777 331.2639
0.1316 -0.0933 324.6510
0.1104 -0.1127 314.4273
0.1003 -0.1292 307.8212
0.0773 -0.1393 299.0228
0.0470 -0.1513 287.2397
0.0304 -0.1559 281.0305
0.0065 -0.1614 272.2953
-0.0202 -0.1596 262.7822
-0.0395 -0.1577 255.9313
-0.0763 -0.1412 241.6018
-0.1067 -0.1265 229.8468
-0.1288 -0.1035 218.7801
-0.1444 -0.0777 208.2825
-0.1610 -0.0519 197.8833
-0.1610 -0.0372 193.0193
-0.1711 -0.0115 183.8321
-0.1711 0.0180 174.0008
-0.1674 0.0391 166.8408
-0.1545 0.0778 153.2824
-0.1380 0.1054 142.6278
-0.1233 0.1201 135.7416
-0.1067 0.1348 128.3564
-0.0966 0.1422 124.1847
-0.0782 0.1569 116.4832
-0.0542 0.1679 107.9021
-0.0276 0.1735 99.0311
-0.0037 0.1753 91.1928
0.0120 0.1716 86.0036
0.0267 0.1689 81.0115
0.0543 0.1633 71.6082
0.0764 0.1523 63.3628
0.0985 0.1376 54.4076
0.1169 0.1201 45.7809
0.1288 0.1045 39.0364
0.1463 0.0805 28.8317
0.1555 0.0640 22.3633
0.1638 0.0327 11.2904
Recuerda que la calibración debe ser en 3D, ya que los UAV no realizan movimientos solo en 2D.
La calibración cuando esta cerca de objetos que perturban el campo magnético de la tierra se hace la misma calibración pero con aquellos elementos.
En cuanto la compensación por inclinación si no me convence como escribí en el otro post. Asi se use los ángulos de inclinación para corregirlos, no se obtiene buenos resultados, llega a los 40 grados(ejem. cabeceo) aprox y a partir de ahí tengo una desviación de hasta 20 grados en azimut(cuando llega a 90 grados en cabeceo).
Espero que alguien me confirme o diga que si funciona para todos los angulos y como lo hizo. :mrgreen:
Ya que en lo que he leído sobre la corrección de inclinación no he visto que hagan pruebas para ángulos pronunciados.
Saludos.
Oye elgarbe, ¿al obtener los datos en Excel (Que no sé cómo lo haces), lo haces moviendo de algún modo al Magnetómetro? ¿O dejas al sensor estático? Tengo esa curiosidad, pues estoy tratando de calibrar el mío. Para calibrarlo en realidad solo hago múltiples mediciones y obtengo los mínimos y máximos de los ejes X y Y. Tomo esa diferencia en los ejes y se la resto a las nuevas mediciones. Pero no logro obtener buenos resultados. Supongo pues que es en la calibración.
Oye elgarbe, ¿al obtener los datos en Excel (Que no sé cómo lo haces), lo haces moviendo de algún modo al Magnetómetro? ¿O dejas al sensor estático? Tengo esa curiosidad, pues estoy tratando de calibrar el mío. Para calibrarlo en realidad solo hago múltiples mediciones y obtengo los mínimos y máximos de los ejes X y Y. Tomo esa diferencia en los ejes y se la resto a las nuevas mediciones. Pero no logro obtener buenos resultados. Supongo pues que es en la calibración.
para calibrar tenes que rotar 360º el sensor en un solo eje y almacenar lo obtenido, luego en otro eje y luego en otro. Luego se grafica en excel y se obtiene el offset en cada eje.
No debe haber objetos metálicos serca ni campos magneticos ni electricos potentes.
sds.
Oye elgarbe, ¿al obtener los datos en Excel (Que no sé cómo lo haces), lo haces moviendo de algún modo al Magnetómetro? ¿O dejas al sensor estático? Tengo esa curiosidad, pues estoy tratando de calibrar el mío. Para calibrarlo en realidad solo hago múltiples mediciones y obtengo los mínimos y máximos de los ejes X y Y. Tomo esa diferencia en los ejes y se la resto a las nuevas mediciones. Pero no logro obtener buenos resultados. Supongo pues que es en la calibración.
para calibrar tenes que rotar 360º el sensor en un solo eje y almacenar lo obtenido, luego en otro eje y luego en otro. Luego se grafica en excel y se obtiene el offset en cada eje.
No debe haber objetos metálicos serca ni campos magneticos ni electricos potentes.
sds.
Gracias por el dato, oye y aprovechando ¿Cómo haces las gráficas X y Y? ¿Qué software utilizas o qué onda?
Ah, por cierto. En tu opinión: ¿Qué objetos afectan la calibración del Magnetómetro? Porque tengo en mi escritorio Bocinas, Celulares, un RadioControl, una Batería y motores DC y Brushless Jaja
Muchas gracias!!!, sí, con la comunicación I2C estoy muy limitado en cuanto a la fracuencia de muestreo, no puedo superar por mucho los 200 Hz, pero esta IMU ya viene así. Acabo de comprar esta IMU, http://www.aliexpress.com/snapshot/6736509118.html?orderId=67829496724044
estoy esperando a que llegue.
No sé si con esa frecuencia de muestreo que estoy logrando pueda hacer una caracterización del sistema para obtener la función de transferencia con MATLAB. Unos compañeros están teniendo problemas para estabilizar el cuadcóptero, pero están unsando el módulo DMP para fusionar los datos, yo quiero hacerlo con Kalman y poder comparar.
¿Qué opinión tienen respecto a la MPU-9250 9-eje actitud + giroscopio + acelerador + magnetómetro? es SPI, por eso la compré, tarda 39 días en llegar a Argentina, mientras tanto experimento con la que tengo, para adquirir experiencia.
Hace poco terminé el curso que dictó la Universidad de Texas en Austin sobre Sistemas Embebidos, ahí conocí la TIVA launchpad, compré algunas en TI y pedí varias muestras del CORTEX, así que tengo para entretenerme.
Ahora que nombrás los DMA, voy a intentar usarlos en el dsPIC -ya que en el curso no vimos el uso del DMA en el Cortex-, que también tengo en todos los colores, jajaja. Por ahora estoy pasando el código al dsPIC33FJ128MC804 que trabaja a 40MIP, pero tengo otros más fuertes que llegan a 70MIPS, pero tengo que hacer las placas. Saludos y gracias por la orientación. Apenas tenga la fusión hecha en Mathematica, comparto el archivo, además necesito estimar las matrices de covarianza de los sensores, para que Kalman funcione mejor. Mathematica hace el trabajo, ya lo hice para el acelerómetro y giróscopo, ahora trabajaré sobre el magnetómetro. Abrazos!!!
HMC5883L_read_data();
gaus_X = (float)mag_X * 0.92/1000 - 0.18305; //Resto el offset obtenido en el Excel
gaus_Y = (float)mag_Y * 0.92/1000 + 0.3271;
gaus_Z = (float)mag_Z * 0.92/1000;
heading = atan2(gaus_Y, gaus_X); //Calculo el ángulo respecto al Norte Magnético
if(heading < 0.0){ //Si el valor es negativo
heading += (2.0 * PI); //sumamos 2*Pi para que no queden cortes en transiciones
} // de 179° a -180°
if(heading > (2.0 * PI)){ // Por si el valor es mayor a 2 PI
heading -= (2.0 * PI);
}
heading *= (180.0 / PI); //Paso a °
ustedes calculan en angulo pitch y roll usando una fusion entre en gyroscopio y el acelerometro. Dandole mayor prioridad a lo que mide el giroscopio verdad? (cerca de un 98%)
¿es muy necesaria la fusion sensorial entre los 2 sensores?
el acelerometro por si solo me da unos angulos bastante aproximados a lo real, es bastante sensible a cualquier vibracion. por el momento estoy redondeando el angulo que mide. Esto lo hago debido a que estoy teniendo problemas de memoria en la targeta que estoy utilizando y hacer dicha fusion sensorial me llena la memoria ram de momento.
¿alguien ha utilizado una c2000 de texas instruments?
tiene una memoria flash bastante grande, de momento no se como compilar en ella. xD
igualmente para el eje Z, medirlo con el giroscopio y arreglarlo con el heading del magnetometro es lo que ustedes comentan.
¿que es el offset del magnetometro y como se calcula?
Gracias por la ayuda.