Diff: FXOS8700CQ.cpp

Revision:

5:2781e519691b

Parent:

3:2ce85aa45d7d

Child:

6:f79ab691a47b

--- a/FXOS8700CQ.cpp	Tue Jun 03 19:02:19 2014 +0000
+++ b/FXOS8700CQ.cpp	Fri Feb 05 00:20:23 2016 +0000
@@ -6,71 +6,11 @@
 // Construct class and its contents
 FXOS8700CQ::FXOS8700CQ(PinName sda, PinName scl, int addr) : dev_i2c(sda, scl), dev_addr(addr)
 {
-    // Initialization of the FXOS8700CQ
+    // Initialization of the I2C interface
     dev_i2c.frequency(I2C_400K); // Use maximum I2C frequency
-    uint8_t data[6] = {0, 0, 0, 0, 0, 0}; // to write over I2C: device register, up to 5 bytes data
-
-    // TODO: verify WHOAMI?
-
-    // Place peripheral in standby for configuration, resetting CTRL_REG1
-    data[0] = FXOS8700CQ_CTRL_REG1;
-    data[1] = 0x00; // this will unset CTRL_REG1:active
-    write_regs(data, 2);
-
-    // Now that the device is in standby, configure registers at will
-
-    // Setup for write-though for CTRL_REG series
-    // Keep data[0] as FXOS8700CQ_CTRL_REG1
-    data[1] =
-        FXOS8700CQ_CTRL_REG1_ASLP_RATE2(1) | // 0b01 gives sleep rate of 12.5Hz
-        FXOS8700CQ_CTRL_REG1_DR3(1); // 0x001 gives ODR of 400Hz/200Hz hybrid
-
-    // FXOS8700CQ_CTRL_REG2;
-    data[2] =
-        FXOS8700CQ_CTRL_REG2_SMODS2(3) | // 0b11 gives low power sleep oversampling mode
-        FXOS8700CQ_CTRL_REG2_MODS2(1); // 0b01 gives low noise, low power oversampling mode
-
-    // No configuration changes from default 0x00 in CTRL_REG3
-    // Interrupts will be active low, their outputs in push-pull mode
-    data[3] = 0x00;
-
-    // FXOS8700CQ_CTRL_REG4;
-    data[4] =
-        FXOS8700CQ_CTRL_REG4_INT_EN_DRDY; // Enable the Data-Ready interrupt
 
-    // No configuration changes from default 0x00 in CTRL_REG5
-    // Data-Ready interrupt will appear on INT2
-    data[5] = 0x00;
-
-    // Write to the 5 CTRL_REG registers
-    write_regs(data, 6);
-
-    // FXOS8700CQ_XYZ_DATA_CFG
-    data[0] = FXOS8700CQ_XYZ_DATA_CFG;
-    data[1] =
-        FXOS8700CQ_XYZ_DATA_CFG_FS2(1); // 0x01 gives 4g full range, 0.488mg/LSB
-    write_regs(data, 2);
-
-    // Setup for write-through for M_CTRL_REG series
-    data[0] = FXOS8700CQ_M_CTRL_REG1;
-    data[1] =
-        FXOS8700CQ_M_CTRL_REG1_M_ACAL | // set automatic calibration
-        FXOS8700CQ_M_CTRL_REG1_MO_OS3(7) | // use maximum magnetic oversampling
-        FXOS8700CQ_M_CTRL_REG1_M_HMS2(3); // enable hybrid sampling (both sensors)
-
-    // FXOS8700CQ_M_CTRL_REG2
-    data[2] =
-        FXOS8700CQ_M_CTRL_REG2_HYB_AUTOINC_MODE;
-
-    // FXOS8700CQ_M_CTRL_REG3
-    data[3] =
-        FXOS8700CQ_M_CTRL_REG3_M_ASLP_OS3(7); // maximum sleep magnetic oversampling
-
-    // Write to the 3 M_CTRL_REG registers
-    write_regs(data, 4);
-
-    // Peripheral is configured, but disabled
-    enabled = false;
+    // Initialization of the IC
+    write_setup();
 }
 
 // Destruct class
@@ -113,34 +53,57 @@
     return databyte;
 }
 
-uint8_t FXOS8700CQ::get_data(SRAWDATA *accel_data, SRAWDATA *magn_data)
+uint8_t FXOS8700CQ::get_data(SRAWDATA *accel_data)//, SRAWDATA *magn_data)
 {
     if(!enabled) {
         return 1;
     }
 
-    read_regs(FXOS8700CQ_M_OUT_X_MSB, raw, FXOS8700CQ_READ_LEN);
+    //read_regs(FXOS8700CQ_M_OUT_X_MSB, raw, FXOS8700CQ_READ_LEN);
+    read_regs(FXOS8700CQ_OUT_X_MSB, raw, FXOS8700CQ_READ_LEN);
 
     // Pull out 16-bit, 2's complement magnetometer data
-    magn_data->x = (raw[0] << 8) | raw[1];
-    magn_data->y = (raw[2] << 8) | raw[3];
-    magn_data->z = (raw[4] << 8) | raw[5];
+//    magn_data->x = (raw[0] << 8) | raw[1];
+//    magn_data->y = (raw[2] << 8) | raw[3];
+//    magn_data->z = (raw[4] << 8) | raw[5];
 
     // Pull out 14-bit, 2's complement, right-justified accelerometer data
-    accel_data->x = (raw[6] << 8) | raw[7];
-    accel_data->y = (raw[8] << 8) | raw[9];
-    accel_data->z = (raw[10] << 8) | raw[11];
+    //accel_data->x = (raw[6] << 8) | raw[7];
+    //accel_data->y = (raw[8] << 8) | raw[9];
+    //accel_data->z = (raw[10] << 8) | raw[11];
+
+    // Pull out 14-bit, 2's complement, *L*E*F*T*-justified accelerometer data
+    // into 16 bit variables
+    accel_data->x = (raw[0] << 8) | raw[1];
+    accel_data->y = (raw[2] << 8) | raw[3];
+    accel_data->z = (raw[4] << 8) | raw[5];
+
+    //deal with -ve representations
+    if (accel_data->x && 0x8000) { //MSB is ONE
+        accel_data->x = -(~(accel_data->x)+1);
+    }
+    if (accel_data->y && 0x8000) { //MSB is ONE
+        accel_data->y = -(~(accel_data->y)+1);
+    }
+    if (accel_data->z && 0x8000) { //MSB is ONE
+        accel_data->z = -(~(accel_data->z)+1);
+    }
+    
+    // scale back down to actual 14 bit number
+    accel_data->x = (accel_data->x)/4;
+    accel_data->y = (accel_data->y)/4;
+    accel_data->z = (accel_data->z)/4;
 
     // Have to apply corrections to make the int16_t correct
-    if(accel_data->x > UINT14_MAX/2) {
-        accel_data->x -= UINT14_MAX;
-    }
-    if(accel_data->y > UINT14_MAX/2) {
-        accel_data->y -= UINT14_MAX;
-    }
-    if(accel_data->z > UINT14_MAX/2) {
-        accel_data->z -= UINT14_MAX;
-    }
+    //if(accel_data->x > UINT14_MAX/2) {
+    //    accel_data->x -= UINT14_MAX;
+    //}
+    //if(accel_data->y > UINT14_MAX/2) {
+    //    accel_data->y -= UINT14_MAX;
+    //}
+    //if(accel_data->z > UINT14_MAX/2) {
+    //    accel_data->z -= UINT14_MAX;
+    //}
 
     return 0;
 }
@@ -164,6 +127,90 @@
     }
 }
 
+uint8_t FXOS8700CQ::read_single_reg(uint8_t RegAddr)
+{
+    uint8_t data = 0x00;
+    read_regs(RegAddr, &data, 1);
+    return data;
+}
+
+uint8_t FXOS8700CQ::write_single_reg(uint8_t RegAddr, uint8_t data)
+{
+    uint8_t  message[2] = {RegAddr, data};
+    write_regs(message, 2);
+    return 1;
+}
+
+uint8_t FXOS8700CQ::write_setup(void)
+{
+    uint8_t data[6] = {0, 0, 0, 0, 0, 0}; // to write over I2C: device register, up to 5 bytes data
+
+    // TODO: verify WHOAMI?
+
+    // Place peripheral in standby for configuration, resetting CTRL_REG1
+    data[0] = FXOS8700CQ_CTRL_REG1;
+    data[1] = 0x00; // this will unset CTRL_REG1:active
+    write_regs(data, 2);
+
+    // Now that the device is in standby, configure registers at will
+
+    // Setup for write-though for CTRL_REG series
+    // Keep data[0] as FXOS8700CQ_CTRL_REG1
+    data[1] = 0x24;  //DR3 = b100  lnoise = 1
+        //FXOS8700CQ_CTRL_REG1_ASLP_RATE2(1) | // 0b01 gives sleep rate of 12.5Hz
+        //FXOS8700CQ_CTRL_REG1_DR3(3); // 0x001 gives ODR of 400Hz/200Hz hybrid
+
+    // FXOS8700CQ_CTRL_REG2;
+    data[2] = 0x12; //SMODS = MODS = 2 - high resolution. nothing else on here
+        //FXOS8700CQ_CTRL_REG2_SMODS2(3) | // 0b11 gives low power sleep oversampling mode
+        //FXOS8700CQ_CTRL_REG2_MODS2(1); // 0b01 gives low noise, low power oversampling mode
+
+    // No configuration changes from default 0x00 in CTRL_REG3
+    // Interrupts will be active low, their outputs in push-pull mode
+    data[3] = 0x00;
+
+    // FXOS8700CQ_CTRL_REG4;
+    data[4] =
+        FXOS8700CQ_CTRL_REG4_INT_EN_DRDY; // Enable the Data-Ready interrupt
+
+    // No configuration changes from default 0x00 in CTRL_REG5
+    // Data-Ready interrupt will appear on INT2
+    data[5] = 0x00;
+
+    // Write to the 5 CTRL_REG registers
+    write_regs(data, 6);
+
+    // FXOS8700CQ_XYZ_DATA_CFG
+    data[0] = FXOS8700CQ_XYZ_DATA_CFG;
+    data[1] =
+        FXOS8700CQ_XYZ_DATA_CFG_FS2(0); //JH 0x00 gives 2g range // 0x01 gives 4g full range, 0.488mg/LSB
+    write_regs(data, 2);
+
+    // Setup for write-through for M_CTRL_REG series
+    data[0] = FXOS8700CQ_M_CTRL_REG1;
+    data[1] = 0x00;
+        //FXOS8700CQ_M_CTRL_REG1_M_ACAL | // set automatic calibration
+        //FXOS8700CQ_M_CTRL_REG1_MO_OS3(7) | // use maximum magnetic oversampling
+        ///FXOS8700CQ_M_CTRL_REG1_M_HMS2(3); // enable hybrid sampling (both sensors)
+
+    // FXOS8700CQ_M_CTRL_REG2
+    data[2] = 0x00;
+        //FXOS8700CQ_M_CTRL_REG2_HYB_AUTOINC_MODE;
+
+    // FXOS8700CQ_M_CTRL_REG3
+    data[3] = 0x00;
+        //FXOS8700CQ_M_CTRL_REG3_M_ASLP_OS3(7); // maximum sleep magnetic oversampling
+
+    // Write to the 3 M_CTRL_REG registers
+    //write_regs(data, 4);
+
+    // Peripheral is configured, but disabled
+    enabled = false;
+    return 1;
+}
+
+
+
 // Private methods
 
 // Excepting the call to dev_i2c.frequency() in the constructor,