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