Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1993 - 1
/*
2
HMC58X3.cpp - Interface a Honeywell HMC58X3 or HMC5883L magnetometer to an Arduino via i2c
3
Copyright (C) 2011 Fabio Varesano <fvaresano@yahoo.it>
4
 
5
Based on:
6
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1274748346
7
 Modification/extension of the following by E.J.Muller
8
http://eclecti.cc/hardware/hmc5843-magnetometer-library-for-arduino
9
 Copyright (c) 2009 Nirav Patel,
10
 
11
The above were based on:
12
http://www.sparkfun.com/datasheets/Sensors/Magneto/HMC58X3-v11.c
13
http://www.atmel.com/dyn/resources/prod_documents/doc2545.pdf
14
 
15
 
16
This program is free software: you can redistribute it and/or modify
17
it under the terms of the version 3 GNU General Public License as
18
published by the Free Software Foundation.
19
 
20
This program is distributed in the hope that it will be useful,
21
but WITHOUT ANY WARRANTY; without even the implied warranty of
22
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
23
GNU General Public License for more details.
24
 
25
You should have received a copy of the GNU General Public License
26
along with this program.  If not, see <http://www.gnu.org/licenses/>.
27
 
28
*/
29
 
30
 
31
#include "WProgram.h"
32
#include <HMC58X3.h>
33
 
34
 
35
/* PUBLIC METHODS */
36
 
37
HMC58X3::HMC58X3() {
38
 
39
  x_scale=1;
40
  y_scale=1;
41
  z_scale=1;
42
}
43
 
44
 
45
void HMC58X3::init(bool setmode) {
46
  // note that we don't initialize Wire here. 
47
  // You'll have to do that in setup() in your Arduino program
48
  delay(5); // you need to wait at least 5ms after power on to initialize
49
  if (setmode) {
50
    setMode(0);
51
  }
52
 
53
  writeReg(HMC58X3_R_CONFA, 0x70);
54
  writeReg(HMC58X3_R_CONFB, 0xA0);
55
  writeReg(HMC58X3_R_MODE, 0x00);
56
}
57
 
58
 
59
void HMC58X3::setMode(unsigned char mode) {
60
  if (mode > 2) {
61
    return;
62
  }
63
 
64
  writeReg(HMC58X3_R_MODE, mode);
65
  delay(100);
66
}
67
 
68
 
69
void HMC58X3::calibrate(unsigned char gain) {
70
  x_scale=1; // get actual values
71
  y_scale=1;
72
  z_scale=1;
73
  writeReg(HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to pos bias
74
  setGain(gain);
75
  float x, y, z, mx=0, my=0, mz=0, t=10;
76
 
77
  for (int i=0; i<(int)t; i++) {
78
    setMode(1);
79
    getValues(&x,&y,&z);
80
    if (x>mx) mx=x;
81
    if (y>my) my=y;
82
    if (z>mz) mz=z;
83
  }
84
 
85
  float max=0;
86
  if (mx>max) max=mx;
87
  if (my>max) max=my;
88
  if (mz>max) max=mz;
89
  x_max=mx;
90
  y_max=my;
91
  z_max=mz;
92
  x_scale=max/mx; // calc scales
93
  y_scale=max/my;
94
  z_scale=max/mz;
95
  writeReg(HMC58X3_R_CONFA, 0x010); // set RegA/DOR back to default
96
}
97
 
98
 
99
// set data output rate
100
// 0-6, 4 default, normal operation assumed
101
void HMC58X3::setDOR(unsigned char DOR) {
102
  if (DOR>6) return;
103
  writeReg(HMC58X3_R_CONFA,DOR<<2);
104
}
105
 
106
 
107
void HMC58X3::setGain(unsigned char gain) {
108
  // 0-7, 1 default
109
  if (gain > 7) return;
110
  writeReg(HMC58X3_R_CONFB, gain << 5);
111
}
112
 
113
 
114
void HMC58X3::writeReg(unsigned char reg, unsigned char val) {
115
  Wire.beginTransmission(HMC58X3_ADDR);
116
  Wire.send(reg);        // send register address
117
  Wire.send(val);        // send value to write
118
  Wire.endTransmission(); //end transmission
119
}
120
 
121
 
122
void HMC58X3::getValues(int *x,int *y,int *z) {
123
  float fx,fy,fz;
124
  getValues(&fx,&fy,&fz);
125
  *x= (int) (fx + 0.5);
126
  *y= (int) (fy + 0.5);
127
  *z= (int) (fz + 0.5);
128
}
129
 
130
 
131
void HMC58X3::getValues(float *x,float *y,float *z) {
132
  int xr,yr,zr;
133
 
134
  getRaw(&xr, &yr, &zr);
135
  *x= ((float) xr) / x_scale;
136
  *y = ((float) yr) / y_scale;
137
  *z = ((float) zr) / z_scale;
138
}
139
 
140
 
141
void HMC58X3::getRaw(int *x,int *y,int *z) {
142
  Wire.beginTransmission(HMC58X3_ADDR);
143
  Wire.send(HMC58X3_R_XM); // will start from DATA X MSB and fetch all the others
144
  Wire.endTransmission();
145
 
146
  Wire.beginTransmission(HMC58X3_ADDR);
147
  Wire.requestFrom(HMC58X3_ADDR, 6);
148
  if(6 == Wire.available()) {
149
    // read out the 3 values, 2 bytes each.
150
    *x = (Wire.receive() << 8) | Wire.receive();
151
    #ifdef ISHMC5843
152
      *y = (Wire.receive() << 8) | Wire.receive();
153
      *z = (Wire.receive() << 8) | Wire.receive();
154
    #else // the Z registers comes before the Y registers in the HMC5883L
155
      *z = (Wire.receive() << 8) | Wire.receive();
156
      *y = (Wire.receive() << 8) | Wire.receive();
157
    #endif
158
    // the HMC58X3 will automatically wrap around on the next request
159
  }
160
  Wire.endTransmission();
161
}
162
 
163
 
164
void HMC58X3::getValues(float *xyz) {
165
  getValues(&xyz[0], &xyz[1], &xyz[2]);
166
}
167