Revision 1:8b53edef272f, committed 2014-04-13

Comitter:

JimCarver

Date:

Sun Apr 13 21:22:58 2014 +0000

Parent:

0:2562215f5bc0

Child:

2:ab84f99086e5

Child:

3:eb1271ef90bc

Commit message:

This version fixes bugs in the previous version

Changed in this revision

--- 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

--- a/FXOS8700Q.h	Mon Apr 07 00:58:50 2014 +0000
+++ b/FXOS8700Q.h	Sun Apr 13 21:22:58 2014 +0000
@@ -3,62 +3,87 @@
 
 #include "mbed.h"
 // FXOS8700CQ I2C address
-#define FXOS8700CQ_SLAVE_ADDR 0x3C // with pins SA0=0, SA1=0
-
+#define FXOS8700CQ_SLAVE_ADDR0 (0x1E<<1) // with pins SA0=0, SA1=0
+#define FXOS8700CQ_SLAVE_ADDR1 (0x1D<<1) // with pins SA0=1, SA1=0
+#define FXOS8700CQ_SLAVE_ADDR2 (0x1C<<1) // with pins SA0=0, SA1=1
+#define FXOS8700CQ_SLAVE_ADDR3 (0x1F<<1) // with pins SA0=1, SA1=1
 // FXOS8700CQ internal register addresses
-#define FXOS8700CQ_STATUS 0x00
-#define FXOS8700CQ_WHOAMI 0x0D
-#define FXOS8700CQ_XYZ_DATA_CFG 0x0E
-#define FXOS8700CQ_CTRL_REG1 0x2A
-#define FXOS8700CQ_M_CTRL_REG1 0x5B
-#define FXOS8700CQ_M_CTRL_REG2 0x5C
-#define FXOS8700CQ_WHOAMI_VAL 0xC7
+#define FXOS8700Q_STATUS 0x00
+#define FXOS8700Q_OUT_X_MSB 0x01
+#define FXOS8700Q_OUT_Y_MSB 0x03
+#define FXOS8700Q_OUT_Z_MSB 0x05
+#define FXOS8700Q_M_OUT_X_MSB 0x33
+#define FXOS8700Q_M_OUT_Y_MSB 0x35
+#define FXOS8700Q_M_OUT_Z_MSB 0x37
+#define FXOS8700Q_WHOAMI 0x0D
+#define FXOS8700Q_XYZ_DATA_CFG 0x0E
+#define FXOS8700Q_CTRL_REG1 0x2A
+#define FXOS8700Q_M_CTRL_REG1 0x5B
+#define FXOS8700Q_M_CTRL_REG2 0x5C
+#define FXOS8700Q_WHOAMI_VAL 0xC7
 
 class FXOS8700Q
 {
 public:
-    /**
-    * MPL3115A2 constructor
-    *
-    * @param sda SDA pin
-    * @param sdl SCL pin
-    * @param addr addr of the I2C peripheral
-    */
-    FXOS8700Q(PinName sda, PinName scl);
-    
-    /**
-    * Get the value of the WHO_AM_I register
-    *
-    * @returns DEVICE_ID value == ??
-    */
-    //uint8_t getDeviceID();
-    
-    /**
-    * Return the STATUS register value
-    *
-    * @returns STATUS register value
-    */
-    //unsigned char getStatus( void);
-    
-    /**
-    * Get the Accelerometer & Magnetometer values
-    * Accelerometer data is in G's
-    * MAgnetometer Data is in microteslas
-    *
-    * @returns altimeter value as float
-    */
-    void ReadXYZ(float * a, float * m);
-      
+  /**
+  * FXOS8700Q constructor
+  *
+  * @param sda SDA pin
+  * @param sdl SCL pin
+  * @param addr addr of the I2C peripheral
+  */
+  
+  FXOS8700Q(PinName sda, PinName scl, int addr);
+
+  /**
+  * FXOS8700Q destructor
+  */
+  ~FXOS8700Q();
+
+  /**
+   * Get the value of the WHO_AM_I register
+   *
+   * @returns WHO_AM_I value
+   */
+  uint8_t getWhoAmI();
+
+  /**
+   * Get X axis acceleration
+   *
+   * @returns X axis acceleration
+   */
+  float getAccX();
 
-private:
+  /**
+   * Get Y axis acceleration
+   *
+   * @returns Y axis acceleration
+   */
+  float getAccY();
+
+  /**
+   * Get Z axis acceleration
+   *
+   * @returns Z axis acceleration
+   */
+  float getAccZ();
 
-    I2C _i2c;
-    /** Set the device in active mode
-    */
-    void begin( void);
-    
-    void RegRead( char reg, char * d, int len);
+  /**
+   * Get XYZ axis acceleration
+   *
+   * @param res array where acceleration data will be stored
+   */
+  void getAccAllAxis(float * res);
+  void AccXYZraw(int16_t * d);
+  void MagXYZraw(int16_t * d);
+  void readRegs(int addr, uint8_t * data, int len);
+  
+private:
+  I2C m_i2c;
+  int m_addr;
 
+  void writeRegs(uint8_t * data, int len);
+  int16_t getAccAxis(uint8_t addr);
 
 };