Yifei Teng / HMC58X3

A quick adaptation of a library made for Arduino by Fabio Varesano Interface a Honeywell HMC58X3 magnetometer to a mbed via i2c.

Dependents:   FreeIMU FreeIMU

Fork of HMC58X3 by Aloïs Wolff

HMC58X3.cpp@8:ca00fa66673a, 2018-04-18 (annotated)

Committer:
tyftyftyf
Date:
Wed Apr 18 20:24:06 2018 +0000
Revision:
8:ca00fa66673a
Parent:
7:5279fb447af1 
change to I2C 1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pommzorz 0:d84e1c7e4e86 1 /*
pommzorz 0:d84e1c7e4e86 2 HMC58X3.cpp - Interface a Honeywell HMC58X3 or HMC5883L magnetometer to an mbed via i2c
pommzorz 0:d84e1c7e4e86 3 Copyright (C) 2011 Fabio Varesano <fvaresano@yahoo.it>
pommzorz 0:d84e1c7e4e86 4 ported for mbed by Aloïs Wolff (wolffalois@gmail.com)
pommzorz 0:d84e1c7e4e86 5
pommzorz 0:d84e1c7e4e86 6 Based on:
pommzorz 0:d84e1c7e4e86 7 http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1274748346
pommzorz 0:d84e1c7e4e86 8 Modification/extension of the following by E.J.Muller
pommzorz 0:d84e1c7e4e86 9 http://eclecti.cc/hardware/hmc5843-magnetometer-library-for-arduino
pommzorz 0:d84e1c7e4e86 10 Copyright (c) 2009 Nirav Patel,
pommzorz 0:d84e1c7e4e86 11
pommzorz 0:d84e1c7e4e86 12 The above were based on:
pommzorz 0:d84e1c7e4e86 13 http://www.sparkfun.com/datasheets/Sensors/Magneto/HMC58X3-v11.c
pommzorz 0:d84e1c7e4e86 14 http://www.atmel.com/dyn/resources/prod_documents/doc2545.pdf
pommzorz 0:d84e1c7e4e86 15
pommzorz 0:d84e1c7e4e86 16
pommzorz 0:d84e1c7e4e86 17 This program is free software: you can redistribute it and/or modify
pommzorz 0:d84e1c7e4e86 18 it under the terms of the version 3 GNU General Public License as
pommzorz 0:d84e1c7e4e86 19 published by the Free Software Foundation.
pommzorz 0:d84e1c7e4e86 20
pommzorz 0:d84e1c7e4e86 21 This program is distributed in the hope that it will be useful,
pommzorz 0:d84e1c7e4e86 22 but WITHOUT ANY WARRANTY; without even the implied warranty of
pommzorz 0:d84e1c7e4e86 23 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
pommzorz 0:d84e1c7e4e86 24 GNU General Public License for more details.
pommzorz 0:d84e1c7e4e86 25
pommzorz 0:d84e1c7e4e86 26 You should have received a copy of the GNU General Public License
pommzorz 0:d84e1c7e4e86 27 along with this program. If not, see <http://www.gnu.org/licenses/>.
pommzorz 0:d84e1c7e4e86 28
pommzorz 0:d84e1c7e4e86 29 */
pommzorz 0:d84e1c7e4e86 30
pommzorz 0:d84e1c7e4e86 31 //#define DEBUG (1)
pommzorz 0:d84e1c7e4e86 32
tyftyftyf 7:5279fb447af1 33 #include "HMC58X3.h"
pommzorz 0:d84e1c7e4e86 34 #include "mbed.h"
tyftyftyf 4:8eb12adc8368 35 #include "MODI2C.h"
tyftyftyf 4:8eb12adc8368 36 #include "rtos.h"
pommzorz 0:d84e1c7e4e86 37 #include <new>
pommzorz 0:d84e1c7e4e86 38 //#include <DebugUtils.h>
pommzorz 0:d84e1c7e4e86 39 #define DEBUG_PRINT
pommzorz 0:d84e1c7e4e86 40
pommzorz 0:d84e1c7e4e86 41 /*!
pommzorz 0:d84e1c7e4e86 42 Counts/milli-gauss per gain for the self test bias current.
pommzorz 0:d84e1c7e4e86 43 */
tyftyftyf 2:c5ac16c88514 44
pommzorz 0:d84e1c7e4e86 45 const int counts_per_milligauss[8]= {
pommzorz 0:d84e1c7e4e86 46 1370,
pommzorz 0:d84e1c7e4e86 47 1090,
pommzorz 0:d84e1c7e4e86 48 820,
pommzorz 0:d84e1c7e4e86 49 660,
pommzorz 0:d84e1c7e4e86 50 440,
pommzorz 0:d84e1c7e4e86 51 390,
pommzorz 0:d84e1c7e4e86 52 330,
pommzorz 0:d84e1c7e4e86 53 230
pommzorz 0:d84e1c7e4e86 54 };
tyftyftyf 2:c5ac16c88514 55
pommzorz 0:d84e1c7e4e86 56
pommzorz 0:d84e1c7e4e86 57
pommzorz 0:d84e1c7e4e86 58
pommzorz 0:d84e1c7e4e86 59 /* PUBLIC METHODS */
pommzorz 0:d84e1c7e4e86 60
tyftyftyf 2:c5ac16c88514 61
pommzorz 1:72ecf7399250 62 //HMC58X3::HMC58X3(PinName sda, PinName scl): i2c(sda, scl)
tyftyftyf 6:02c679492d35 63 HMC58X3::HMC58X3():i2c(I2C_SDA,I2C_SCL),_thread(osPriorityAboveNormal),sem(0)
pommzorz 0:d84e1c7e4e86 64 {
pommzorz 1:72ecf7399250 65 //this->i2c= i2c_;
pommzorz 0:d84e1c7e4e86 66 x_scale=1.0F;
pommzorz 0:d84e1c7e4e86 67 y_scale=1.0F;
pommzorz 0:d84e1c7e4e86 68 z_scale=1.0F;
pommzorz 0:d84e1c7e4e86 69
tyftyftyf 4:8eb12adc8368 70 HMC58X3_R_IDA = 10;
tyftyftyf 6:02c679492d35 71
tyftyftyf 6:02c679492d35 72 _thread.start(callback(&HMC58X3::samplingthread_stub, (void *) this));
pommzorz 0:d84e1c7e4e86 73 }
pommzorz 0:d84e1c7e4e86 74
tyftyftyf 4:8eb12adc8368 75 void HMC58X3::samplingthread_stub(void const *p) {
tyftyftyf 4:8eb12adc8368 76 HMC58X3 *instance = (HMC58X3*)p;
tyftyftyf 4:8eb12adc8368 77 instance->samplingthread();
tyftyftyf 4:8eb12adc8368 78 }
pommzorz 0:d84e1c7e4e86 79
pommzorz 0:d84e1c7e4e86 80 void HMC58X3::init(bool setmode)
pommzorz 0:d84e1c7e4e86 81 {
pommzorz 0:d84e1c7e4e86 82 // note that we don't initialize Wire here.
pommzorz 0:d84e1c7e4e86 83 // You'll have to do that in setup() in your Arduino program
tyftyftyf 4:8eb12adc8368 84 Thread::wait(10); // you need to wait at least 5ms after power on to initialize
tyftyftyf 4:8eb12adc8368 85
pommzorz 0:d84e1c7e4e86 86 if (setmode) {
pommzorz 0:d84e1c7e4e86 87 setMode(0);
pommzorz 0:d84e1c7e4e86 88 }
pommzorz 0:d84e1c7e4e86 89
tyftyftyf 4:8eb12adc8368 90 writeReg(HMC58X3_R_CONFA, 0x18); // 4 samples averaged, 75Hz frequency, no artificial bias.
tyftyftyf 4:8eb12adc8368 91
pommzorz 0:d84e1c7e4e86 92 writeReg(HMC58X3_R_CONFB, 0xA0);
tyftyftyf 4:8eb12adc8368 93
pommzorz 0:d84e1c7e4e86 94 writeReg(HMC58X3_R_MODE, 0x00);
pommzorz 0:d84e1c7e4e86 95 }
pommzorz 0:d84e1c7e4e86 96
pommzorz 0:d84e1c7e4e86 97
pommzorz 0:d84e1c7e4e86 98 void HMC58X3::setMode(unsigned char mode)
pommzorz 0:d84e1c7e4e86 99 {
pommzorz 0:d84e1c7e4e86 100 if (mode > 2) {
pommzorz 0:d84e1c7e4e86 101 return;
pommzorz 0:d84e1c7e4e86 102 }
pommzorz 0:d84e1c7e4e86 103
pommzorz 0:d84e1c7e4e86 104 writeReg(HMC58X3_R_MODE, mode);
tyftyftyf 4:8eb12adc8368 105 Thread::wait(100);
pommzorz 0:d84e1c7e4e86 106 }
pommzorz 0:d84e1c7e4e86 107
pommzorz 0:d84e1c7e4e86 108 /*
pommzorz 0:d84e1c7e4e86 109 Calibrate which has a few weaknesses.
pommzorz 0:d84e1c7e4e86 110 1. Uses wrong gain for first reading.
pommzorz 0:d84e1c7e4e86 111 2. Uses max instead of max of average when normalizing the axis to one another.
pommzorz 0:d84e1c7e4e86 112 3. Doesn't use neg bias. (possible improvement in measurement).
pommzorz 0:d84e1c7e4e86 113 */
pommzorz 0:d84e1c7e4e86 114 void HMC58X3::calibrate(unsigned char gain)
pommzorz 0:d84e1c7e4e86 115 {
pommzorz 0:d84e1c7e4e86 116 x_scale=1; // get actual values
pommzorz 0:d84e1c7e4e86 117 y_scale=1;
pommzorz 0:d84e1c7e4e86 118 z_scale=1;
pommzorz 0:d84e1c7e4e86 119 writeReg(HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to pos bias
pommzorz 0:d84e1c7e4e86 120 setGain(gain);
pommzorz 0:d84e1c7e4e86 121 float x, y, z, mx=0, my=0, mz=0, t=10;
pommzorz 0:d84e1c7e4e86 122
pommzorz 0:d84e1c7e4e86 123 for (int i=0; i<(int)t; i++) {
pommzorz 0:d84e1c7e4e86 124 setMode(1);
pommzorz 0:d84e1c7e4e86 125 getValues(&x,&y,&z);
pommzorz 0:d84e1c7e4e86 126 if (x>mx) mx=x;
pommzorz 0:d84e1c7e4e86 127 if (y>my) my=y;
pommzorz 0:d84e1c7e4e86 128 if (z>mz) mz=z;
pommzorz 0:d84e1c7e4e86 129 }
pommzorz 0:d84e1c7e4e86 130
pommzorz 0:d84e1c7e4e86 131 float max=0;
pommzorz 0:d84e1c7e4e86 132 if (mx>max) max=mx;
pommzorz 0:d84e1c7e4e86 133 if (my>max) max=my;
pommzorz 0:d84e1c7e4e86 134 if (mz>max) max=mz;
pommzorz 0:d84e1c7e4e86 135 x_max=mx;
pommzorz 0:d84e1c7e4e86 136 y_max=my;
pommzorz 0:d84e1c7e4e86 137 z_max=mz;
pommzorz 0:d84e1c7e4e86 138 x_scale=max/mx; // calc scales
pommzorz 0:d84e1c7e4e86 139 y_scale=max/my;
pommzorz 0:d84e1c7e4e86 140 z_scale=max/mz;
pommzorz 0:d84e1c7e4e86 141
pommzorz 0:d84e1c7e4e86 142 writeReg(HMC58X3_R_CONFA, 0x010); // set RegA/DOR back to default
pommzorz 0:d84e1c7e4e86 143 } // calibrate().
pommzorz 0:d84e1c7e4e86 144
pommzorz 0:d84e1c7e4e86 145 /*!
pommzorz 0:d84e1c7e4e86 146 \brief Calibrate using the self test operation.
pommzorz 0:d84e1c7e4e86 147
pommzorz 0:d84e1c7e4e86 148 Average the values using bias mode to obtain the scale factors.
pommzorz 0:d84e1c7e4e86 149
pommzorz 0:d84e1c7e4e86 150 \param gain [in] Gain setting for the sensor. See data sheet.
pommzorz 0:d84e1c7e4e86 151 \param n_samples [in] Number of samples to average together while applying the positive and negative bias.
pommzorz 0:d84e1c7e4e86 152 \return Returns false if any of the following occurs:
pommzorz 0:d84e1c7e4e86 153 # Invalid input parameters. (gain>7 or n_samples=0).
pommzorz 0:d84e1c7e4e86 154 # Id registers are wrong for the compiled device. Unfortunately, we can't distinguish between HMC5843 and HMC5883L.
pommzorz 0:d84e1c7e4e86 155 # Calibration saturates during the positive or negative bias on any of the readings.
pommzorz 0:d84e1c7e4e86 156 # Readings are outside of the expected range for bias current.
pommzorz 0:d84e1c7e4e86 157 */
pommzorz 0:d84e1c7e4e86 158 bool HMC58X3::calibrate(unsigned char gain,unsigned int n_samples)
pommzorz 0:d84e1c7e4e86 159 {
tyftyftyf 2:c5ac16c88514 160 int16_t xyz[3]; // 16 bit integer values for each axis.
tyftyftyf 2:c5ac16c88514 161 long xyz_total[3]= {0,0,0}; // 32 bit totals so they won't overflow.
pommzorz 0:d84e1c7e4e86 162 bool bret=true; // Function return value. Will return false if the wrong identifier is returned, saturation is detected or response is out of range to self test bias.
pommzorz 0:d84e1c7e4e86 163 char id[3]; // Three identification registers should return 'H43'.
tyftyftyf 2:c5ac16c88514 164 long low_limit, high_limit;
pommzorz 0:d84e1c7e4e86 165 /*
pommzorz 0:d84e1c7e4e86 166 Make sure we are talking to the correct device.
pommzorz 0:d84e1c7e4e86 167 Hard to believe Honeywell didn't change the identifier.
pommzorz 0:d84e1c7e4e86 168 */
pommzorz 0:d84e1c7e4e86 169 if ((8>gain) && (0<n_samples)) { // Notice this allows gain setting of 7 which the data sheet warns against.
pommzorz 0:d84e1c7e4e86 170 getID(id);
pommzorz 0:d84e1c7e4e86 171 if (('H' == id[0]) && ('4' == id[1]) && ('3' == id[2])) {
pommzorz 0:d84e1c7e4e86 172 /*
pommzorz 0:d84e1c7e4e86 173 Use the positive bias current to impose a known field on each axis.
pommzorz 0:d84e1c7e4e86 174 This field depends on the device and the axis.
pommzorz 0:d84e1c7e4e86 175 */
pommzorz 0:d84e1c7e4e86 176 writeReg(HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to pos bias
pommzorz 0:d84e1c7e4e86 177 /*
pommzorz 0:d84e1c7e4e86 178 Note that the very first measurement after a gain change maintains the same gain as the previous setting.
pommzorz 0:d84e1c7e4e86 179 The new gain setting is effective from the second measurement and on.
pommzorz 0:d84e1c7e4e86 180 */
pommzorz 0:d84e1c7e4e86 181 setGain(gain);
pommzorz 0:d84e1c7e4e86 182 setMode(1); // Change to single measurement mode.
pommzorz 0:d84e1c7e4e86 183 getRaw(&xyz[0],&xyz[1],&xyz[2]); // Get the raw values and ignore since this reading may use previous gain.
pommzorz 0:d84e1c7e4e86 184
pommzorz 0:d84e1c7e4e86 185 for (unsigned int i=0; i<n_samples; i++) {
pommzorz 0:d84e1c7e4e86 186 setMode(1);
pommzorz 0:d84e1c7e4e86 187 getRaw(&xyz[0],&xyz[1],&xyz[2]); // Get the raw values in case the scales have already been changed.
pommzorz 0:d84e1c7e4e86 188 /*
pommzorz 0:d84e1c7e4e86 189 Since the measurements are noisy, they should be averaged rather than taking the max.
pommzorz 0:d84e1c7e4e86 190 */
pommzorz 0:d84e1c7e4e86 191 xyz_total[0]+=xyz[0];
pommzorz 0:d84e1c7e4e86 192 xyz_total[1]+=xyz[1];
pommzorz 0:d84e1c7e4e86 193 xyz_total[2]+=xyz[2];
pommzorz 0:d84e1c7e4e86 194 /*
pommzorz 0:d84e1c7e4e86 195 Detect saturation.
pommzorz 0:d84e1c7e4e86 196 */
pommzorz 0:d84e1c7e4e86 197 if (-(1<<12) >= min(xyz[0],min(xyz[1],xyz[2]))) {
pommzorz 0:d84e1c7e4e86 198 DEBUG_PRINT("HMC58x3 Self test saturated. Increase range.");
pommzorz 0:d84e1c7e4e86 199 bret=false;
pommzorz 0:d84e1c7e4e86 200 break; // Breaks out of the for loop. No sense in continuing if we saturated.
pommzorz 0:d84e1c7e4e86 201 }
pommzorz 0:d84e1c7e4e86 202 }
pommzorz 0:d84e1c7e4e86 203 /*
pommzorz 0:d84e1c7e4e86 204 Apply the negative bias. (Same gain)
pommzorz 0:d84e1c7e4e86 205 */
pommzorz 0:d84e1c7e4e86 206 writeReg(HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to negative bias.
pommzorz 0:d84e1c7e4e86 207 for (unsigned int i=0; i<n_samples; i++) {
pommzorz 0:d84e1c7e4e86 208 setMode(1);
pommzorz 0:d84e1c7e4e86 209 getRaw(&xyz[0],&xyz[1],&xyz[2]); // Get the raw values in case the scales have already been changed.
pommzorz 0:d84e1c7e4e86 210 /*
pommzorz 0:d84e1c7e4e86 211 Since the measurements are noisy, they should be averaged.
pommzorz 0:d84e1c7e4e86 212 */
pommzorz 0:d84e1c7e4e86 213 xyz_total[0]-=xyz[0];
pommzorz 0:d84e1c7e4e86 214 xyz_total[1]-=xyz[1];
pommzorz 0:d84e1c7e4e86 215 xyz_total[2]-=xyz[2];
pommzorz 0:d84e1c7e4e86 216 /*
pommzorz 0:d84e1c7e4e86 217 Detect saturation.
pommzorz 0:d84e1c7e4e86 218 */
pommzorz 0:d84e1c7e4e86 219 if (-(1<<12) >= min(xyz[0],min(xyz[1],xyz[2]))) {
pommzorz 0:d84e1c7e4e86 220 DEBUG_PRINT("HMC58x3 Self test saturated. Increase range.");
pommzorz 0:d84e1c7e4e86 221 bret=false;
pommzorz 0:d84e1c7e4e86 222 break; // Breaks out of the for loop. No sense in continuing if we saturated.
pommzorz 0:d84e1c7e4e86 223 }
pommzorz 0:d84e1c7e4e86 224 }
pommzorz 0:d84e1c7e4e86 225 /*
pommzorz 0:d84e1c7e4e86 226 Compare the values against the expected self test bias gauss.
pommzorz 0:d84e1c7e4e86 227 Notice, the same limits are applied to all axis.
pommzorz 0:d84e1c7e4e86 228 */
pommzorz 0:d84e1c7e4e86 229 low_limit =SELF_TEST_LOW_LIMIT *counts_per_milligauss[gain]*2*n_samples;
pommzorz 0:d84e1c7e4e86 230 high_limit=SELF_TEST_HIGH_LIMIT*counts_per_milligauss[gain]*2*n_samples;
pommzorz 0:d84e1c7e4e86 231
pommzorz 0:d84e1c7e4e86 232 if ((true==bret) &&
pommzorz 0:d84e1c7e4e86 233 (low_limit <= xyz_total[0]) && (high_limit >= xyz_total[0]) &&
pommzorz 0:d84e1c7e4e86 234 (low_limit <= xyz_total[1]) && (high_limit >= xyz_total[1]) &&
pommzorz 0:d84e1c7e4e86 235 (low_limit <= xyz_total[2]) && (high_limit >= xyz_total[2]) ) {
pommzorz 0:d84e1c7e4e86 236 /*
pommzorz 0:d84e1c7e4e86 237 Successful calibration.
pommzorz 0:d84e1c7e4e86 238 Normalize the scale factors so all axis return the same range of values for the bias field.
pommzorz 0:d84e1c7e4e86 239 Factor of 2 is from summation of total of n_samples from both positive and negative bias.
pommzorz 0:d84e1c7e4e86 240 */
pommzorz 0:d84e1c7e4e86 241 x_scale=(counts_per_milligauss[gain]*(HMC58X3_X_SELF_TEST_GAUSS*2))/(xyz_total[0]/n_samples);
pommzorz 0:d84e1c7e4e86 242 y_scale=(counts_per_milligauss[gain]*(HMC58X3_Y_SELF_TEST_GAUSS*2))/(xyz_total[1]/n_samples);
pommzorz 0:d84e1c7e4e86 243 z_scale=(counts_per_milligauss[gain]*(HMC58X3_Z_SELF_TEST_GAUSS*2))/(xyz_total[2]/n_samples);
pommzorz 0:d84e1c7e4e86 244 } else {
pommzorz 0:d84e1c7e4e86 245 DEBUG_PRINT("HMC58x3 Self test out of range.");
pommzorz 0:d84e1c7e4e86 246 bret=false;
pommzorz 0:d84e1c7e4e86 247 }
pommzorz 0:d84e1c7e4e86 248 writeReg(HMC58X3_R_CONFA, 0x010); // set RegA/DOR back to default.
pommzorz 0:d84e1c7e4e86 249 } else {
pommzorz 0:d84e1c7e4e86 250 #if defined(ISHMC5843)
pommzorz 0:d84e1c7e4e86 251 DEBUG_PRINT("HMC5843 failed id check.");
pommzorz 0:d84e1c7e4e86 252 #else
pommzorz 0:d84e1c7e4e86 253 DEBUG_PRINT("HMC5883L failed id check.");
pommzorz 0:d84e1c7e4e86 254 #endif
pommzorz 0:d84e1c7e4e86 255 bret=false;
pommzorz 0:d84e1c7e4e86 256 }
pommzorz 0:d84e1c7e4e86 257 } else {
pommzorz 0:d84e1c7e4e86 258 /*
pommzorz 0:d84e1c7e4e86 259 Bad input parameters.
pommzorz 0:d84e1c7e4e86 260 */
pommzorz 0:d84e1c7e4e86 261 DEBUG_PRINT("HMC58x3 Bad parameters.");
pommzorz 0:d84e1c7e4e86 262 bret=false;
pommzorz 0:d84e1c7e4e86 263 }
pommzorz 0:d84e1c7e4e86 264 return(bret);
pommzorz 0:d84e1c7e4e86 265 } // calibrate().
pommzorz 0:d84e1c7e4e86 266
pommzorz 0:d84e1c7e4e86 267 // set data output rate
pommzorz 0:d84e1c7e4e86 268 // 0-6, 4 default, normal operation assumed
pommzorz 0:d84e1c7e4e86 269 void HMC58X3::setDOR(unsigned char DOR)
pommzorz 0:d84e1c7e4e86 270 {
pommzorz 0:d84e1c7e4e86 271 if (DOR>6) return;
pommzorz 0:d84e1c7e4e86 272 writeReg(HMC58X3_R_CONFA,DOR<<2);
pommzorz 0:d84e1c7e4e86 273 }
pommzorz 0:d84e1c7e4e86 274
pommzorz 0:d84e1c7e4e86 275
pommzorz 0:d84e1c7e4e86 276 void HMC58X3::setGain(unsigned char gain)
pommzorz 0:d84e1c7e4e86 277 {
pommzorz 0:d84e1c7e4e86 278 // 0-7, 1 default
pommzorz 0:d84e1c7e4e86 279 if (gain > 7) return;
pommzorz 0:d84e1c7e4e86 280 writeReg(HMC58X3_R_CONFB, gain << 5);
pommzorz 0:d84e1c7e4e86 281 }
pommzorz 0:d84e1c7e4e86 282
tyftyftyf 4:8eb12adc8368 283 MemoryPool<short, 16> mpool;
tyftyftyf 4:8eb12adc8368 284
tyftyftyf 4:8eb12adc8368 285 uint32_t writeregfin(uint32_t in)
tyftyftyf 4:8eb12adc8368 286 {
tyftyftyf 4:8eb12adc8368 287 short *tmp = (short *)in;
tyftyftyf 4:8eb12adc8368 288 mpool.free(tmp);
tyftyftyf 4:8eb12adc8368 289 return 0;
tyftyftyf 4:8eb12adc8368 290 }
pommzorz 0:d84e1c7e4e86 291
pommzorz 0:d84e1c7e4e86 292 void HMC58X3::writeReg(unsigned char reg, unsigned char val)
pommzorz 0:d84e1c7e4e86 293 {
tyftyftyf 4:8eb12adc8368 294 unsigned char *tmp = (unsigned char *)mpool.alloc();
tyftyftyf 4:8eb12adc8368 295 tmp[0]=reg;
tyftyftyf 4:8eb12adc8368 296 tmp[1]=val;
tyftyftyf 4:8eb12adc8368 297 i2c.write(I2C_ADDRESS, (char*)tmp, 2, &writeregfin, (void*)tmp);
pommzorz 0:d84e1c7e4e86 298 }
pommzorz 0:d84e1c7e4e86 299
pommzorz 0:d84e1c7e4e86 300
tyftyftyf 2:c5ac16c88514 301 void HMC58X3::getValues(int16_t *x,int16_t *y,int16_t *z)
pommzorz 0:d84e1c7e4e86 302 {
pommzorz 0:d84e1c7e4e86 303 float fx,fy,fz;
pommzorz 0:d84e1c7e4e86 304 getValues(&fx,&fy,&fz);
tyftyftyf 2:c5ac16c88514 305 *x= (int16_t) (fx + 0.5);
tyftyftyf 2:c5ac16c88514 306 *y= (int16_t) (fy + 0.5);
tyftyftyf 2:c5ac16c88514 307 *z= (int16_t) (fz + 0.5);
pommzorz 0:d84e1c7e4e86 308 }
pommzorz 0:d84e1c7e4e86 309
pommzorz 0:d84e1c7e4e86 310
pommzorz 0:d84e1c7e4e86 311 void HMC58X3::getValues(float *x,float *y,float *z)
pommzorz 0:d84e1c7e4e86 312 {
tyftyftyf 2:c5ac16c88514 313 int16_t xr,yr,zr;
pommzorz 0:d84e1c7e4e86 314
pommzorz 0:d84e1c7e4e86 315 getRaw(&xr, &yr, &zr);
pommzorz 0:d84e1c7e4e86 316 *x= ((float) xr) / x_scale;
pommzorz 0:d84e1c7e4e86 317 *y = ((float) yr) / y_scale;
pommzorz 0:d84e1c7e4e86 318 *z = ((float) zr) / z_scale;
pommzorz 0:d84e1c7e4e86 319 }
pommzorz 0:d84e1c7e4e86 320
tyftyftyf 4:8eb12adc8368 321 uint32_t magn_readfin(uint32_t param){
tyftyftyf 4:8eb12adc8368 322 HMC58X3* ins = (HMC58X3*)param;
tyftyftyf 4:8eb12adc8368 323 ins->sem.release();
tyftyftyf 4:8eb12adc8368 324 return 0;
tyftyftyf 4:8eb12adc8368 325 }
tyftyftyf 4:8eb12adc8368 326
tyftyftyf 4:8eb12adc8368 327 void HMC58X3::start_sampling(){
tyftyftyf 4:8eb12adc8368 328 _thread.signal_set(0x1);
tyftyftyf 4:8eb12adc8368 329 }
tyftyftyf 4:8eb12adc8368 330
tyftyftyf 4:8eb12adc8368 331 bool magn_valid = false;
tyftyftyf 4:8eb12adc8368 332
tyftyftyf 4:8eb12adc8368 333 void HMC58X3::samplingthread()
tyftyftyf 4:8eb12adc8368 334 {
tyftyftyf 4:8eb12adc8368 335 char tmp[2];
tyftyftyf 4:8eb12adc8368 336 tmp[0]=HMC58X3_R_MODE;
tyftyftyf 4:8eb12adc8368 337 tmp[1]=1;
tyftyftyf 4:8eb12adc8368 338 char magn_data[6];
tyftyftyf 4:8eb12adc8368 339 Timer magntimer;
tyftyftyf 4:8eb12adc8368 340
tyftyftyf 4:8eb12adc8368 341 char cmd[2];
tyftyftyf 4:8eb12adc8368 342 cmd[0] = 0x03;
tyftyftyf 4:8eb12adc8368 343
tyftyftyf 4:8eb12adc8368 344 Thread::signal_wait(0x1);
tyftyftyf 4:8eb12adc8368 345 magntimer.start();
tyftyftyf 4:8eb12adc8368 346
tyftyftyf 4:8eb12adc8368 347 for (;;) {
tyftyftyf 4:8eb12adc8368 348 i2c.write(0x3D, (char*)tmp, 2);
tyftyftyf 4:8eb12adc8368 349
tyftyftyf 4:8eb12adc8368 350 i2c.write(0x3D, cmd, 1, true); // set the pointer to the start of x
tyftyftyf 4:8eb12adc8368 351
tyftyftyf 4:8eb12adc8368 352 magntimer.reset();
tyftyftyf 4:8eb12adc8368 353
tyftyftyf 4:8eb12adc8368 354 i2c.read_nb(0x3D, (char*)magn_data, 6, &magn_readfin, this, false);
tyftyftyf 4:8eb12adc8368 355
tyftyftyf 4:8eb12adc8368 356 sem.wait();
tyftyftyf 4:8eb12adc8368 357
tyftyftyf 4:8eb12adc8368 358 // read out the 3 values, 2 bytes each.
tyftyftyf 4:8eb12adc8368 359 cache_x = int16_t(((unsigned char)magn_data[0] << 8) | (unsigned char)magn_data[1]);
tyftyftyf 4:8eb12adc8368 360 #ifdef ISHMC5843
tyftyftyf 4:8eb12adc8368 361 cache_y = int16_t(((unsigned char)magn_data[1*2] << 8) | (unsigned char)magn_data[1*2+1]);
tyftyftyf 4:8eb12adc8368 362 cache_z = int16_t(((unsigned char)magn_data[2*2] << 8) | (unsigned char)magn_data[2*2+1]);
tyftyftyf 4:8eb12adc8368 363 #else // the Z registers comes before the Y registers in the HMC5883L
tyftyftyf 4:8eb12adc8368 364 cache_z = int16_t(((unsigned char)magn_data[1*2] << 8) | (unsigned char)magn_data[1*2+1]);
tyftyftyf 4:8eb12adc8368 365 cache_y = int16_t(((unsigned char)magn_data[2*2] << 8) | (unsigned char)magn_data[2*2+1]);
tyftyftyf 4:8eb12adc8368 366 #endif
tyftyftyf 4:8eb12adc8368 367 // the HMC58X3 will automatically wrap around on the next request.
tyftyftyf 4:8eb12adc8368 368
tyftyftyf 4:8eb12adc8368 369 magn_valid = true;
tyftyftyf 4:8eb12adc8368 370
tyftyftyf 4:8eb12adc8368 371 int time = ((6800-magntimer.read_us())/1000);
tyftyftyf 4:8eb12adc8368 372 if (time >= 0)
tyftyftyf 4:8eb12adc8368 373 Thread::wait(time);
tyftyftyf 4:8eb12adc8368 374 }
tyftyftyf 4:8eb12adc8368 375
tyftyftyf 4:8eb12adc8368 376 }
pommzorz 0:d84e1c7e4e86 377
tyftyftyf 2:c5ac16c88514 378 void HMC58X3::getRaw(int16_t *x,int16_t *y,int16_t *z)
pommzorz 0:d84e1c7e4e86 379 {
tyftyftyf 4:8eb12adc8368 380 *x = cache_x;
tyftyftyf 4:8eb12adc8368 381 *y = cache_y;
tyftyftyf 4:8eb12adc8368 382 *z = cache_z;
pommzorz 0:d84e1c7e4e86 383 }
pommzorz 0:d84e1c7e4e86 384
pommzorz 0:d84e1c7e4e86 385
pommzorz 0:d84e1c7e4e86 386 void HMC58X3::getValues(float *xyz)
pommzorz 0:d84e1c7e4e86 387 {
pommzorz 0:d84e1c7e4e86 388 getValues(&xyz[0], &xyz[1], &xyz[2]);
pommzorz 0:d84e1c7e4e86 389 }
pommzorz 0:d84e1c7e4e86 390
pommzorz 0:d84e1c7e4e86 391 /*!
pommzorz 0:d84e1c7e4e86 392 \brief Retrieve the value of the three ID registers.
pommzorz 0:d84e1c7e4e86 393
pommzorz 0:d84e1c7e4e86 394 Note: Both the HMC5843 and HMC5883L have the same 'H43' identification register values. (Looks like someone at Honeywell screwed up.)
pommzorz 0:d84e1c7e4e86 395
pommzorz 0:d84e1c7e4e86 396 \param id [out] Returns the three id register values.
pommzorz 0:d84e1c7e4e86 397 */
pommzorz 0:d84e1c7e4e86 398 void HMC58X3::getID(char id[3])
pommzorz 0:d84e1c7e4e86 399 {
tyftyftyf 4:8eb12adc8368 400 i2c.write(I2C_ADDRESS, (char*)&HMC58X3_R_IDA, 1, true);
tyftyftyf 4:8eb12adc8368 401 i2c.read(I2C_ADDRESS, id, 3);
pommzorz 0:d84e1c7e4e86 402
pommzorz 0:d84e1c7e4e86 403 } // getID().
pommzorz 0:d84e1c7e4e86 404
pommzorz 0:d84e1c7e4e86 405 int HMC58X3::min (int a, int b)
pommzorz 0:d84e1c7e4e86 406 {
pommzorz 0:d84e1c7e4e86 407 return !(b<a)?a:b; // or: return !comp(b,a)?a:b; for version (2)
pommzorz 0:d84e1c7e4e86 408 }