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