WebSVN - Projects - Diff - Rev 2194 and 2198 - /Transportables


/*
hmc5883l lib 0x01
 
copyright (c) Davide Gironi, 2012
 
Released under GPLv3.
Please refer to LICENSE file for licensing information.
*/
//############################################################################
//# HISTORY  hmc5883l.c
//#
//# 19.09.2015 cebra
//# - add: Library für HMC5883L Compass, GY-87 Sensorboard
//#
//############################################################################
 
 
#ifdef USE_KOMPASS
#include <stdlib.h>
#include <avr/io.h>
#include <util/delay.h>
 
#include "hmc5883l.h"
 
//path to i2c fleury lib
#include HMC5883L_I2CFLEURYPATH
 
static double hmc5883l_scale = 0;
 
/*
 * init the hmc5883l
 */
void hmc5883l_init() {
        #if HMC5883L_I2CINIT == 1
        //init i2c
        i2c_init();
        _delay_us(10);
        #endif
 
        //set scale
        hmc5883l_scale = 0;
        uint8_t regValue = 0x00;
        #if HMC5883L_SCALE == HMC5883L_SCALE088
                regValue = 0x00;
                hmc5883l_scale = 0.73;
        #elif HMC5883L_SCALE == HMC5883L_SCALE13
                regValue = 0x01;
                hmc5883l_scale = 0.92;
        #elif HMC5883L_SCALE == HMC5883L_SCALE19
                regValue = 0x02;
                hmc5883l_scale = 1.22;
        #elif HMC5883L_SCALE == HMC5883L_SCALE25
                regValue = 0x03;
                hmc5883l_scale = 1.52;
        #elif HMC5883L_SCALE == HMC5883L_SCALE40
                regValue = 0x04;
                hmc5883l_scale = 2.27;
        #elif HMC5883L_SCALE == HMC5883L_SCALE47
                regValue = 0x05;
                hmc5883l_scale = 2.56;
        #elif HMC5883L_SCALE == HMC5883L_SCALE56
                regValue = 0x06;
                hmc5883l_scale = 3.03;
        #elif HMC5883L_SCALE == HMC5883L_SCALE81
                regValue = 0x07;
                hmc5883l_scale = 4.35;
        #endif
 
        //setting is in the top 3 bits of the register.
        regValue = regValue << 5;
    i2c_start_wait(HMC5883L_ADDR | I2C_WRITE);
    i2c_write(HMC5883L_CONFREGB);
    i2c_write(regValue);
    i2c_stop();
 
        //set measurement mode
        i2c_start_wait(HMC5883L_ADDR | I2C_WRITE);
        i2c_write(HMC5883L_MODEREG);
        i2c_write(HMC5883L_MEASUREMODE);
        i2c_stop();
}
 
/*
 * get raw data
 */
void hmc5883l_getrawdata(int16_t *mxraw, int16_t *myraw, int16_t *mzraw) {
        uint8_t i = 0;
        uint8_t buff[6];
 
        i2c_start_wait(HMC5883L_ADDR | I2C_WRITE);
        i2c_write(HMC5883L_DATAREGBEGIN);
        i2c_stop();
        i2c_start_wait(HMC5883L_ADDR | I2C_READ);
        for(i=0; i<6; i++) {
                if(i==6-1)
                        buff[i] = i2c_readNak();
                else
                        buff[i] = i2c_readAck();
        }
        i2c_stop();
 
        *mxraw = (int16_t)((buff[0] << 8) | buff[1]);
        *mzraw = (int16_t)((buff[2] << 8) | buff[3]);
        *myraw = (int16_t)((buff[4] << 8) | buff[5]);
}
 
/*
 * get scaled data
 */
void hmc5883l_getdata(double *mx, double *my, double *mz) {
        int16_t mxraw = 0;
        int16_t myraw = 0;
        int16_t mzraw = 0;
        hmc5883l_getrawdata(&mxraw, &myraw, &mzraw);
 
        #if HMC5883L_CALIBRATED == 1
        float mxt = mxraw - HMC5883L_OFFSETX;
        float myt = myraw - HMC5883L_OFFSETY;
        float mzt = mzraw - HMC5883L_OFFSETZ;
        *mx = HMC5883L_GAINX1 * mxt + HMC5883L_GAINX2 * myt + HMC5883L_GAINX3 * mzt;
        *my = HMC5883L_GAINY1 * mxt + HMC5883L_GAINY2 * myt + HMC5883L_GAINY3 * mzt;
        *mz = HMC5883L_GAINZ1 * mxt + HMC5883L_GAINZ2 * myt + HMC5883L_GAINZ3 * mzt;
        #else
        *mx = mxraw * hmc5883l_scale;
        *my = myraw * hmc5883l_scale;
        *mz = mzraw * hmc5883l_scale;
        #endif
 
 
 
}
#endif
 

Rev 2194	Rev 2198
1	/*	1	/*
2	hmc5883l lib 0x01	2	hmc5883l lib 0x01
3		3
4	copyright (c) Davide Gironi, 2012	4	copyright (c) Davide Gironi, 2012
5		5
6	Released under GPLv3.	6	Released under GPLv3.
7	Please refer to LICENSE file for licensing information.	7	Please refer to LICENSE file for licensing information.
8	*/	8	*/
-		9	//############################################################################
-		10	//# HISTORY hmc5883l.c
-		11	//#
-		12	//# 19.09.2015 cebra
-		13	//# - add: Library für HMC5883L Compass, GY-87 Sensorboard
-		14	//#
-		15	//############################################################################
9		-
-		16
10		17	#ifdef USE_KOMPASS
11	#include <stdlib.h>	18	#include <stdlib.h>
12	#include <avr/io.h>	19	#include <avr/io.h>
13	#include <util/delay.h>	20	#include <util/delay.h>
14		21
15	#include "hmc5883l.h"	22	#include "hmc5883l.h"
16		23
17	//path to i2c fleury lib	24	//path to i2c fleury lib
18	#include HMC5883L_I2CFLEURYPATH	25	#include HMC5883L_I2CFLEURYPATH
19		26
20	static double hmc5883l_scale = 0;	27	static double hmc5883l_scale = 0;
21		28
22	/*	29	/*
23	* init the hmc5883l	30	* init the hmc5883l
24	*/	31	*/
25	void hmc5883l_init() {	32	void hmc5883l_init() {
26	#if HMC5883L_I2CINIT == 1	33	#if HMC5883L_I2CINIT == 1
27	//init i2c	34	//init i2c
28	i2c_init();	35	i2c_init();
29	_delay_us(10);	36	_delay_us(10);
30	#endif	37	#endif
31		38
32	//set scale	39	//set scale
33	hmc5883l_scale = 0;	40	hmc5883l_scale = 0;
34	uint8_t regValue = 0x00;	41	uint8_t regValue = 0x00;
35	#if HMC5883L_SCALE == HMC5883L_SCALE088	42	#if HMC5883L_SCALE == HMC5883L_SCALE088
36	regValue = 0x00;	43	regValue = 0x00;
37	hmc5883l_scale = 0.73;	44	hmc5883l_scale = 0.73;
38	#elif HMC5883L_SCALE == HMC5883L_SCALE13	45	#elif HMC5883L_SCALE == HMC5883L_SCALE13
39	regValue = 0x01;	46	regValue = 0x01;
40	hmc5883l_scale = 0.92;	47	hmc5883l_scale = 0.92;
41	#elif HMC5883L_SCALE == HMC5883L_SCALE19	48	#elif HMC5883L_SCALE == HMC5883L_SCALE19
42	regValue = 0x02;	49	regValue = 0x02;
43	hmc5883l_scale = 1.22;	50	hmc5883l_scale = 1.22;
44	#elif HMC5883L_SCALE == HMC5883L_SCALE25	51	#elif HMC5883L_SCALE == HMC5883L_SCALE25
45	regValue = 0x03;	52	regValue = 0x03;
46	hmc5883l_scale = 1.52;	53	hmc5883l_scale = 1.52;
47	#elif HMC5883L_SCALE == HMC5883L_SCALE40	54	#elif HMC5883L_SCALE == HMC5883L_SCALE40
48	regValue = 0x04;	55	regValue = 0x04;
49	hmc5883l_scale = 2.27;	56	hmc5883l_scale = 2.27;
50	#elif HMC5883L_SCALE == HMC5883L_SCALE47	57	#elif HMC5883L_SCALE == HMC5883L_SCALE47
51	regValue = 0x05;	58	regValue = 0x05;
52	hmc5883l_scale = 2.56;	59	hmc5883l_scale = 2.56;
53	#elif HMC5883L_SCALE == HMC5883L_SCALE56	60	#elif HMC5883L_SCALE == HMC5883L_SCALE56
54	regValue = 0x06;	61	regValue = 0x06;
55	hmc5883l_scale = 3.03;	62	hmc5883l_scale = 3.03;
56	#elif HMC5883L_SCALE == HMC5883L_SCALE81	63	#elif HMC5883L_SCALE == HMC5883L_SCALE81
57	regValue = 0x07;	64	regValue = 0x07;
58	hmc5883l_scale = 4.35;	65	hmc5883l_scale = 4.35;
59	#endif	66	#endif
60		67
61	//setting is in the top 3 bits of the register.	68	//setting is in the top 3 bits of the register.
62	regValue = regValue << 5;	69	regValue = regValue << 5;
63	i2c_start_wait(HMC5883L_ADDR \| I2C_WRITE);	70	i2c_start_wait(HMC5883L_ADDR \| I2C_WRITE);
64	i2c_write(HMC5883L_CONFREGB);	71	i2c_write(HMC5883L_CONFREGB);
65	i2c_write(regValue);	72	i2c_write(regValue);
66	i2c_stop();	73	i2c_stop();
67		74
68	//set measurement mode	75	//set measurement mode
69	i2c_start_wait(HMC5883L_ADDR \| I2C_WRITE);	76	i2c_start_wait(HMC5883L_ADDR \| I2C_WRITE);
70	i2c_write(HMC5883L_MODEREG);	77	i2c_write(HMC5883L_MODEREG);
71	i2c_write(HMC5883L_MEASUREMODE);	78	i2c_write(HMC5883L_MEASUREMODE);
72	i2c_stop();	79	i2c_stop();
73	}	80	}
74		81
75	/*	82	/*
76	* get raw data	83	* get raw data
77	*/	84	*/
78	void hmc5883l_getrawdata(int16_t mxraw, int16_t myraw, int16_t *mzraw) {	85	void hmc5883l_getrawdata(int16_t mxraw, int16_t myraw, int16_t *mzraw) {
79	uint8_t i = 0;	86	uint8_t i = 0;
80	uint8_t buff[6];	87	uint8_t buff[6];
81		88
82	i2c_start_wait(HMC5883L_ADDR \| I2C_WRITE);	89	i2c_start_wait(HMC5883L_ADDR \| I2C_WRITE);
83	i2c_write(HMC5883L_DATAREGBEGIN);	90	i2c_write(HMC5883L_DATAREGBEGIN);
84	i2c_stop();	91	i2c_stop();
85	i2c_start_wait(HMC5883L_ADDR \| I2C_READ);	92	i2c_start_wait(HMC5883L_ADDR \| I2C_READ);
86	for(i=0; i<6; i++) {	93	for(i=0; i<6; i++) {
87	if(i==6-1)	94	if(i==6-1)
88	buff[i] = i2c_readNak();	95	buff[i] = i2c_readNak();
89	else	96	else
90	buff[i] = i2c_readAck();	97	buff[i] = i2c_readAck();
91	}	98	}
92	i2c_stop();	99	i2c_stop();
93		100
94	*mxraw = (int16_t)((buff[0] << 8) \| buff[1]);	101	*mxraw = (int16_t)((buff[0] << 8) \| buff[1]);
95	*mzraw = (int16_t)((buff[2] << 8) \| buff[3]);	102	*mzraw = (int16_t)((buff[2] << 8) \| buff[3]);
96	*myraw = (int16_t)((buff[4] << 8) \| buff[5]);	103	*myraw = (int16_t)((buff[4] << 8) \| buff[5]);
97	}	104	}
98		105
99	/*	106	/*
100	* get scaled data	107	* get scaled data
101	*/	108	*/
102	void hmc5883l_getdata(double mx, double my, double *mz) {	109	void hmc5883l_getdata(double mx, double my, double *mz) {
103	int16_t mxraw = 0;	110	int16_t mxraw = 0;
104	int16_t myraw = 0;	111	int16_t myraw = 0;
105	int16_t mzraw = 0;	112	int16_t mzraw = 0;
106	hmc5883l_getrawdata(&mxraw, &myraw, &mzraw);	113	hmc5883l_getrawdata(&mxraw, &myraw, &mzraw);
107		114
108	#if HMC5883L_CALIBRATED == 1	115	#if HMC5883L_CALIBRATED == 1
109	float mxt = mxraw - HMC5883L_OFFSETX;	116	float mxt = mxraw - HMC5883L_OFFSETX;
110	float myt = myraw - HMC5883L_OFFSETY;	117	float myt = myraw - HMC5883L_OFFSETY;
111	float mzt = mzraw - HMC5883L_OFFSETZ;	118	float mzt = mzraw - HMC5883L_OFFSETZ;
112	mx = HMC5883L_GAINX1 mxt + HMC5883L_GAINX2 * myt + HMC5883L_GAINX3 * mzt;	119	mx = HMC5883L_GAINX1 mxt + HMC5883L_GAINX2 * myt + HMC5883L_GAINX3 * mzt;
113	my = HMC5883L_GAINY1 mxt + HMC5883L_GAINY2 * myt + HMC5883L_GAINY3 * mzt;	120	my = HMC5883L_GAINY1 mxt + HMC5883L_GAINY2 * myt + HMC5883L_GAINY3 * mzt;
114	mz = HMC5883L_GAINZ1 mxt + HMC5883L_GAINZ2 * myt + HMC5883L_GAINZ3 * mzt;	121	mz = HMC5883L_GAINZ1 mxt + HMC5883L_GAINZ2 * myt + HMC5883L_GAINZ3 * mzt;
115	#else	122	#else
116	mx = mxraw hmc5883l_scale;	123	mx = mxraw hmc5883l_scale;
117	my = myraw hmc5883l_scale;	124	my = myraw hmc5883l_scale;
118	mz = mzraw hmc5883l_scale;	125	mz = mzraw hmc5883l_scale;
119	#endif	126	#endif
120		127
121		128
122		129
123	}	130	}
-		131	#endif
124		132

Subversion Repositories Projects

(root)/Transportables_Koptertool/PKT/trunk/10DOF/hmc5883l.c - Rev 2194 → 2198