Diff: FXOS8700Q.cpp

Revision:

1:8b53edef272f

Parent:

0:2562215f5bc0

Child:

2:ab84f99086e5

Child:

3:eb1271ef90bc

diff -r 2562215f5bc0 -r 8b53edef272f FXOS8700Q.cpp
--- a/FXOS8700Q.cpp	Mon Apr 07 00:58:50 2014 +0000
+++ b/FXOS8700Q.cpp	Sun Apr 13 21:22:58 2014 +0000
@@ -1,94 +1,102 @@
 #include "FXOS8700Q.h"
+#define UINT14_MAX        16383
+
+
+FXOS8700Q::FXOS8700Q(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
+    // activate the peripheral
+    uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_M_CTRL_REG1;
+    data[1] = 0x1F;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_M_CTRL_REG2;
+    data[1] = 0x20;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_XYZ_DATA_CFG;
+    data[1] = 0x00;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_CTRL_REG1;
+    data[1] = 0x19;//0x1D;
+    writeRegs(data, 2);
+}
 
 
-FXOS8700Q::FXOS8700Q(PinName sda, PinName scl) : _i2c(sda, scl) {
- 
-    begin();
+FXOS8700Q::~FXOS8700Q() { }
+
+uint8_t FXOS8700Q::getWhoAmI() {
+    uint8_t who_am_i = 0;
+    readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
+    return who_am_i;
+}
+
+float FXOS8700Q::getAccX() {
+    return (float(getAccAxis(FXOS8700Q_OUT_X_MSB))/4096.0);
 }
-        
-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);
+
+float FXOS8700Q::getAccY() {
+    return (float(getAccAxis(FXOS8700Q_OUT_Y_MSB))/4096.0);
+}
+
+float FXOS8700Q::getAccZ() {
+    return (float(getAccAxis(FXOS8700Q_OUT_Z_MSB))/4096.0);
+}
+
+
+void FXOS8700Q::getAccAllAxis(float * res) {
+    res[0] = getAccX();
+    res[1] = getAccY();
+    res[2] = getAccZ();
 }
 
-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);
+void FXOS8700Q::AccXYZraw(int16_t * d) {
+    int16_t acc;
+    uint8_t res[6];
+    readRegs(FXOS8700Q_OUT_X_MSB, res, 6);
 
-    // 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);
+    acc = (res[0] << 6) | (res[1] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    d[0] = acc;
+    acc = (res[2] << 6) | (res[3] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    d[1] = acc;
+    acc = (res[4] << 6) | (res[5] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    d[2] = acc;
 }
 
-void FXOS8700Q::ReadXYZ(float * a, float * m)
-{
-    char d[13];
-    int16_t t[6];
+void FXOS8700Q::MagXYZraw(int16_t * d) {
+    int16_t acc;
+    uint8_t res[6];
+    readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
+
+    d[0] = (res[0] << 8) | res[1];
+    d[1] = (res[2] << 8) | res[3]; 
+    d[2] = (res[4] << 8) | res[5];
+}
+
+int16_t FXOS8700Q::getAccAxis(uint8_t addr) {
+    int16_t acc;
+    uint8_t res[2];
+    readRegs(addr, res, 2);
 
-    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]));
+    acc = (res[0] << 6) | (res[1] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+
+    return acc;
+}
 
-    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;   
+void FXOS8700Q::readRegs(int addr, uint8_t * data, int len) {
+    char t[1] = {addr};
+    m_i2c.write(m_addr, t, 1, true);
+    m_i2c.read(m_addr, (char *)data, len);
 }
+
+void FXOS8700Q::writeRegs(uint8_t * data, int len) {
+    m_i2c.write(m_addr, (char *)data, len);
+}
+
     
\ No newline at end of file