HMC58X3 Library
Fork of HMC58X3 by
Diff: HMC58X3.cpp
- Revision:
- 4:8eb12adc8368
- Parent:
- 3:1e0e0c47287a
- Child:
- 5:ea869bfe993b
diff -r 1e0e0c47287a -r 8eb12adc8368 HMC58X3.cpp --- a/HMC58X3.cpp Tue Nov 05 11:30:31 2013 +0000 +++ b/HMC58X3.cpp Sat Nov 09 08:51:00 2013 +0000 @@ -31,14 +31,13 @@ //#define DEBUG (1) #include "mbed.h" +#include "MODI2C.h" #include "HMC58X3.h" +#include "rtos.h" #include <new> //#include <DebugUtils.h> #define DEBUG_PRINT - - - /*! Counts/milli-gauss per gain for the self test bias current. */ @@ -61,29 +60,35 @@ //HMC58X3::HMC58X3(PinName sda, PinName scl): i2c(sda, scl) -HMC58X3::HMC58X3(I2C i2c_):i2c(i2c_) +HMC58X3::HMC58X3():i2c(I2C_SDA,I2C_SCL),_thread(&HMC58X3::samplingthread_stub, this, osPriorityAboveNormal),sem(0) { //this->i2c= i2c_; x_scale=1.0F; y_scale=1.0F; z_scale=1.0F; + HMC58X3_R_IDA = 10; } +void HMC58X3::samplingthread_stub(void const *p) { + HMC58X3 *instance = (HMC58X3*)p; + instance->samplingthread(); +} void HMC58X3::init(bool setmode) { // note that we don't initialize Wire here. // You'll have to do that in setup() in your Arduino program - wait_ms(10); // you need to wait at least 5ms after power on to initialize + Thread::wait(10); // you need to wait at least 5ms after power on to initialize + if (setmode) { setMode(0); } - - - writeReg(HMC58X3_R_CONFA, 0x70); // 8 samples averaged, 75Hz frequency, no artificial bias. + writeReg(HMC58X3_R_CONFA, 0x18); // 4 samples averaged, 75Hz frequency, no artificial bias. + writeReg(HMC58X3_R_CONFB, 0xA0); + writeReg(HMC58X3_R_MODE, 0x00); } @@ -95,7 +100,7 @@ } writeReg(HMC58X3_R_MODE, mode); - wait_ms(100); + Thread::wait(100); } /* @@ -273,14 +278,21 @@ writeReg(HMC58X3_R_CONFB, gain << 5); } +MemoryPool<short, 16> mpool; + +uint32_t writeregfin(uint32_t in) +{ + short *tmp = (short *)in; + mpool.free(tmp); + return 0; +} void HMC58X3::writeReg(unsigned char reg, unsigned char val) { - i2c.start(); - i2c.write(HMC58X3_ADDR<<1); - i2c.write(reg); // send register address - i2c.write(val); // send value to write - i2c.stop(); //end transmission + unsigned char *tmp = (unsigned char *)mpool.alloc(); + tmp[0]=reg; + tmp[1]=val; + i2c.write(I2C_ADDRESS, (char*)tmp, 2, &writeregfin, (void*)tmp); } @@ -304,29 +316,68 @@ *z = ((float) zr) / z_scale; } +uint32_t magn_readfin(uint32_t param){ + HMC58X3* ins = (HMC58X3*)param; + ins->sem.release(); + return 0; +} + +void HMC58X3::start_sampling(){ + _thread.signal_set(0x1); +} + +bool magn_valid = false; + +void HMC58X3::samplingthread() +{ + char tmp[2]; + tmp[0]=HMC58X3_R_MODE; + tmp[1]=1; + char magn_data[6]; + Timer magntimer; + + char cmd[2]; + cmd[0] = 0x03; + + Thread::signal_wait(0x1); + magntimer.start(); + + for (;;) { + i2c.write(0x3D, (char*)tmp, 2); + + i2c.write(0x3D, cmd, 1, true); // set the pointer to the start of x + + magntimer.reset(); + + i2c.read_nb(0x3D, (char*)magn_data, 6, &magn_readfin, this, false); + + sem.wait(); + + // read out the 3 values, 2 bytes each. + cache_x = int16_t(((unsigned char)magn_data[0] << 8) | (unsigned char)magn_data[1]); +#ifdef ISHMC5843 + cache_y = int16_t(((unsigned char)magn_data[1*2] << 8) | (unsigned char)magn_data[1*2+1]); + cache_z = int16_t(((unsigned char)magn_data[2*2] << 8) | (unsigned char)magn_data[2*2+1]); +#else // the Z registers comes before the Y registers in the HMC5883L + cache_z = int16_t(((unsigned char)magn_data[1*2] << 8) | (unsigned char)magn_data[1*2+1]); + cache_y = int16_t(((unsigned char)magn_data[2*2] << 8) | (unsigned char)magn_data[2*2+1]); +#endif + // the HMC58X3 will automatically wrap around on the next request. + + magn_valid = true; + + int time = ((6800-magntimer.read_us())/1000); + if (time >= 0) + Thread::wait(time); + } + +} void HMC58X3::getRaw(int16_t *x,int16_t *y,int16_t *z) { - - char cmd[2]; - char data[6]; - cmd[0] = 0x03; - - i2c.write(HMC58X3_ADDR<<1, cmd, 1, true); // set the pointer to the start of x - i2c.read((HMC58X3_ADDR<<1)+1, data, 6, false); - - // read out the 3 values, 2 bytes each. - *x = int16_t(((unsigned char)data[0] << 8) | (unsigned char)data[1]); -#ifdef ISHMC5843 - *y = int16_t(((unsigned char)data[1*2] << 8) | (unsigned char)data[1*2+1]); - *z = int16_t(((unsigned char)data[2*2] << 8) | (unsigned char)data[2*2+1]); -#else // the Z registers comes before the Y registers in the HMC5883L - *z = int16_t(((unsigned char)data[1*2] << 8) | (unsigned char)data[1*2+1]); - *y = int16_t(((unsigned char)data[2*2] << 8) | (unsigned char)data[2*2+1]); -#endif - // the HMC58X3 will automatically wrap around on the next request - - + *x = cache_x; + *y = cache_y; + *z = cache_z; } @@ -344,17 +395,9 @@ */ void HMC58X3::getID(char id[3]) { - i2c.start(); - i2c.write(HMC58X3_ADDR<<1); - i2c.write(HMC58X3_R_IDA); // Will start reading registers starting from Identification Register A. + i2c.write(I2C_ADDRESS, (char*)&HMC58X3_R_IDA, 1, true); + i2c.read(I2C_ADDRESS, id, 3); - i2c.start(); - i2c.write((HMC58X3_ADDR<<1)+1); - id[0] = i2c.read(0); - id[1] = i2c.read(0); - id[2] = i2c.read(0); - - i2c.stop(); } // getID(). int HMC58X3::min (int a, int b)