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 |