Diff: FXOS8700Q.cpp

Revision:

0:2562215f5bc0

Child:

1:8b53edef272f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FXOS8700Q.cpp	Mon Apr 07 00:58:50 2014 +0000
@@ -0,0 +1,94 @@
+#include "FXOS8700Q.h"
+
+
+FXOS8700Q::FXOS8700Q(PinName sda, PinName scl) : _i2c(sda, scl) {
+ 
+    begin();
+}
+        
+void FXOS8700Q::RegRead( char reg, char * d, int len) {
+    char cmd[1];
+    cmd[0] = reg;
+    char i2c_addr = FXOS8700CQ_SLAVE_ADDR;
+    _i2c.write( i2c_addr, cmd, 1);
+    _i2c.read ( i2c_addr, d, len);
+}
+
+void FXOS8700Q::begin(void)
+{
+    char data[2];
+    // write 0000 0000 = 0x00 to accelerometer control register 1 to place FXOS8700CQ into
+    // standby
+    // [7-1] = 0000 000
+    // [0]: active=0
+    data[0] = FXOS8700CQ_CTRL_REG1;
+    data[1] = 0x00;
+    _i2c.write( FXOS8700CQ_SLAVE_ADDR, data, 2);
+
+    // write 0001 1111 = 0x1F to magnetometer control register 1
+    // [7]: m_acal=0: auto calibration disabled
+    // [6]: m_rst=0: no one-shot magnetic reset
+    // [5]: m_ost=0: no one-shot magnetic measurement
+    // [4-2]: m_os=111=7: 8x oversampling (for 200Hz) to reduce magnetometer noise
+    // [1-0]: m_hms=11=3: select hybrid mode with accel and magnetometer active
+    data[0] = FXOS8700CQ_M_CTRL_REG1;
+    data[1] = 0x03; //0x1F;
+    _i2c.write( FXOS8700CQ_SLAVE_ADDR, data, 2);
+    
+    // write 0010 0000 = 0x20 to magnetometer control register 2
+    // [7]: reserved
+    // [6]: reserved
+    // [5]: hyb_autoinc_mode=1 to map the magnetometer registers to follow the
+    // accelerometer registers
+    // [4]: m_maxmin_dis=0 to retain default min/max latching even though not used
+    // [3]: m_maxmin_dis_ths=0
+    // [2]: m_maxmin_rst=0
+    // [1-0]: m_rst_cnt=00 to enable magnetic reset each cycle
+    data[0] = FXOS8700CQ_M_CTRL_REG2;
+    data[1] = 0x20;
+    _i2c.write( FXOS8700CQ_SLAVE_ADDR, data, 2);
+    
+    // write 0000 0001= 0x01 to XYZ_DATA_CFG register
+    // [7]: reserved
+    // [6]: reserved
+    // [5]: reserved
+    // [4]: hpf_out=0
+    // [3]: reserved
+    // [2]: reserved
+    // [1-0]: fs=00 for accelerometer range of +/-2g range with 0.244mg/LSB
+    data[0] = FXOS8700CQ_XYZ_DATA_CFG;
+    data[1] = 0x00;
+    _i2c.write( FXOS8700CQ_SLAVE_ADDR, data, 2);
+
+    // write 0000 1101 = 0x0D to accelerometer control register 1
+    // [7-6]: aslp_rate=00
+    // [5-3]: dr=011 for 50Hz data rate (when in hybrid mode)
+    // [2]: lnoise=1 for low noise mode
+    // [1]: f_read=0 for normal 16 bit reads
+    // [0]: active=1 to take the part out of standby and enable sampling
+    data[0] = FXOS8700CQ_CTRL_REG1;
+    data[1] = 0x19;//0x1D;
+    _i2c.write( FXOS8700CQ_SLAVE_ADDR, data, 2);
+}
+
+void FXOS8700Q::ReadXYZ(float * a, float * m)
+{
+    char d[13];
+    int16_t t[6];
+
+    RegRead( FXOS8700CQ_STATUS, d, 13);
+    t[0] = ((d[1] * 256) + ((unsigned short) d[2]));
+    t[1] = ((d[3] * 256) + ((unsigned short) d[4]));
+    t[2] = ((d[5] * 256) + ((unsigned short) d[6]));
+    t[3] = ((d[7] * 256) + ((unsigned short) d[8]));
+    t[4] = ((d[9] * 256) + ((unsigned short) d[10]));
+    t[5] = ((d[11] * 256) + ((unsigned short) d[12]));
+
+    a[0] = (float) t[0] / 16384.0;
+    a[1] = (float) t[1] / 16384.0;
+    a[2] = (float) t[2] / 16384.0;
+    m[0] = (float) t[3] / 10.0;
+    m[1] = (float) t[4] / 10.0;
+    m[2] = (float) t[5] / 10.0;   
+}
+    
\ No newline at end of file