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