Diff: HMC58X3.cpp

Revision:

4:8eb12adc8368

Parent:

3:1e0e0c47287a

Child:

5:ea869bfe993b

Child:

6:02c679492d35

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)