FXOS8700CQ.cpp

00001 #include "FXOS8700CQ.h"
00002 
00003 uint8_t status_reg; // Status register contents
00004 uint8_t raw[FXOS8700CQ_READ_LEN]; // Buffer for reading out stored data
00005 
00006 // Construct class and its contents
00007 FXOS8700CQ::FXOS8700CQ(PinName sda, PinName scl, int addr) : dev_i2c(sda, scl), dev_addr(addr)
00008 {
00009     accel_data = new SRAWDATA();
00010     magn_data = new SRAWDATA();
00011     
00012     // Initialization of the FXOS8700CQ
00013     dev_i2c.frequency(I2C_400K); // Use maximum I2C frequency
00014     uint8_t data[6] = {0, 0, 0, 0, 0, 0}; // target device write address, single byte to write at address
00015 
00016     // TODO: verify WHOAMI?
00017 
00018     // TODO: un-magic-number register configuration
00019 
00020     // Place peripheral in standby for configuration, resetting CTRL_REG1
00021     data[0] = FXOS8700CQ_CTRL_REG1;
00022     data[1] = 0x00; // this will unset CTRL_REG1:active
00023     write_regs(data, 2);
00024 
00025     // Now that the device is in standby, configure registers
00026 
00027     // Setup for write-though for CTRL_REG series
00028     // Keep data[0] as FXOS8700CQ_CTRL_REG1
00029     data[1] =
00030         FXOS8700CQ_CTRL_REG1_ASLP_RATE2(1) | // 0b01 gives sleep rate of 12.5Hz
00031         FXOS8700CQ_CTRL_REG1_DR3(1); // 0x001 gives ODR of 400Hz/200Hz hybrid
00032 
00033     // FXOS8700CQ_CTRL_REG2;
00034     data[2] =
00035         FXOS8700CQ_CTRL_REG2_SMODS2(3) | // 0b11 gives low power sleep oversampling mode
00036         FXOS8700CQ_CTRL_REG2_MODS2(1); // 0b01 gives low noise, low power oversampling mode
00037 
00038     // No configuration changes from default 0x00 in CTRL_REG3
00039     // Interrupts will be active low
00040     data[3] = 0x00;
00041 
00042     // FXOS8700CQ_CTRL_REG4;
00043     data[4] =
00044         FXOS8700CQ_CTRL_REG4_INT_EN_DRDY;
00045 
00046     // No configuration changes from default 0x00 in CTRL_REG5
00047     // Data ready interrupt will appear on INT2
00048     data[5] = 0x00;
00049 
00050     // Write to the 5 CTRL_REG registers
00051     write_regs(data, 6);
00052 
00053     // FXOS8700CQ_XYZ_DATA_CFG
00054     data[0] = FXOS8700CQ_XYZ_DATA_CFG;
00055     data[1] =
00056         FXOS8700CQ_XYZ_DATA_CFG_FS2(1); // 0x01 gives 4g full range, 0.488mg/LSB
00057     write_regs(data, 2);
00058 
00059     // Setup for write-through for M_CTRL_REG series
00060     data[0] = FXOS8700CQ_M_CTRL_REG1;
00061     data[1] =
00062         FXOS8700CQ_M_CTRL_REG1_M_ACAL | // set automatic calibration
00063         FXOS8700CQ_M_CTRL_REG1_MO_OS3(7) | // use maximum magnetic oversampling
00064         FXOS8700CQ_M_CTRL_REG1_M_HMS2(3); // enable hybrid sampling (both sensors)
00065 
00066     // FXOS8700CQ_M_CTRL_REG2
00067     data[2] =
00068         FXOS8700CQ_M_CTRL_REG2_HYB_AUTOINC_MODE;
00069 
00070     // FXOS8700CQ_M_CTRL_REG3
00071     data[3] =
00072         FXOS8700CQ_M_CTRL_REG3_M_ASLP_OS3(7); // maximum sleep magnetic oversampling
00073 
00074     // Write to the 3 M_CTRL_REG registers
00075     write_regs(data, 4);
00076 
00077     // Peripheral is configured, but disabled
00078     enabled = false;
00079 }
00080 
00081 // Destruct class
00082 FXOS8700CQ::~FXOS8700CQ(void) {
00083     if (accel_data) delete accel_data;
00084     if (magn_data) delete magn_data;
00085 }
00086 
00087 
00088 void FXOS8700CQ::enable(void)
00089 {
00090     uint8_t data[2];
00091     read_regs( FXOS8700CQ_CTRL_REG1, &data[1], 1);
00092     data[1] |= FXOS8700CQ_CTRL_REG1_ACTIVE;
00093     data[0] = FXOS8700CQ_CTRL_REG1;
00094     write_regs(data, 2); // write back
00095 
00096     enabled = true;
00097 }
00098 
00099 void FXOS8700CQ::disable(void)
00100 {
00101     uint8_t data[2];
00102     read_regs( FXOS8700CQ_CTRL_REG1, &data[1], 1);
00103     data[0] = FXOS8700CQ_CTRL_REG1;
00104     data[1] &= ~FXOS8700CQ_CTRL_REG1_ACTIVE;
00105     write_regs(data, 2); // write back
00106 
00107     enabled = false;
00108 }
00109 
00110 
00111 uint8_t FXOS8700CQ::status(void)
00112 {
00113     read_regs(FXOS8700CQ_STATUS, &status_reg, 1);
00114     return status_reg;
00115 }
00116 
00117 uint8_t FXOS8700CQ::get_whoami(void)
00118 {
00119     uint8_t databyte = 0x00;
00120     read_regs(FXOS8700CQ_WHOAMI, &databyte, 1);
00121     return databyte;
00122 }
00123 
00124 uint8_t FXOS8700CQ::get_data()
00125 {
00126     if(!enabled) {
00127         return 1;
00128     }
00129 
00130     read_regs(FXOS8700CQ_M_OUT_X_MSB, raw, FXOS8700CQ_READ_LEN);
00131 
00132     // Pull out 16-bit, 2's complement magnetometer data
00133     magn_data->x = (raw[0] << 8) | raw[1];
00134     magn_data->y = (raw[2] << 8) | raw[3];
00135     magn_data->z = (raw[4] << 8) | raw[5];
00136 
00137     // Pull out 14-bit, 2's complement, right-justified accelerometer data
00138     accel_data->x = (raw[6] << 8) | raw[7];
00139     accel_data->y = (raw[8] << 8) | raw[9];
00140     accel_data->z = (raw[10] << 8) | raw[11];
00141 
00142     // Have to apply corrections to make the int16_t correct
00143     if(accel_data->x > UINT14_MAX/2) {
00144         accel_data->x -= UINT14_MAX;
00145     }
00146     if(accel_data->y > UINT14_MAX/2) {
00147         accel_data->y -= UINT14_MAX;
00148     }
00149     if(accel_data->z > UINT14_MAX/2) {
00150         accel_data->z -= UINT14_MAX;
00151     }
00152 
00153     return 0;
00154 }
00155 
00156 int16_t FXOS8700CQ::getAccelX() { return accel_data->x; }
00157 int16_t FXOS8700CQ::getAccelY() { return accel_data->y; }
00158 int16_t FXOS8700CQ::getAccelZ() { return accel_data->z; }
00159 
00160 int16_t FXOS8700CQ::getMagnetX() { return magn_data->x; }
00161 int16_t FXOS8700CQ::getMagnetY() { return magn_data->y; }
00162 int16_t FXOS8700CQ::getMagnetZ() { return magn_data->z; }
00163 
00164 uint8_t FXOS8700CQ::get_accel_scale(void)
00165 {
00166     uint8_t data = 0x00;
00167     read_regs(FXOS8700CQ_XYZ_DATA_CFG, &data, 1);
00168     data &= FXOS8700CQ_XYZ_DATA_CFG_FS2(3); // mask with 0b11
00169 
00170     switch(data) {
00171         case FXOS8700CQ_XYZ_DATA_CFG_FS2(0):
00172             return 2;
00173         case FXOS8700CQ_XYZ_DATA_CFG_FS2(1):
00174             return 4;
00175         case FXOS8700CQ_XYZ_DATA_CFG_FS2(2):
00176             return 8;
00177         default:
00178             return 0;
00179     }
00180 }
00181 
00182 // Private methods
00183 
00184 void FXOS8700CQ::read_regs(int reg_addr, uint8_t* data, int len)
00185 {
00186     char t[1] = {reg_addr};
00187     dev_i2c.write(dev_addr, t, 1, true);
00188     dev_i2c.read(dev_addr, (char *)data, len);
00189 }
00190 
00191 void FXOS8700CQ::write_regs(uint8_t* data, int len)
00192 {
00193     dev_i2c.write(dev_addr, (char*)data, len);
00194 }