Fork of trm's FXOS8700CQ library
Fork of FXOS8700CQ by
Embed:
(wiki syntax)
Show/hide line numbers
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_ASLP_RATE2(1) | // 0b01 gives sleep rate of 12.5Hz 00026 FXOS8700CQ_CTRL_REG1_DR3(1); // 0x001 gives ODR of 400Hz/200Hz hybrid 00027 00028 // FXOS8700CQ_CTRL_REG2; 00029 data[2] = 00030 FXOS8700CQ_CTRL_REG2_SMODS2(3) | // 0b11 gives low power sleep oversampling mode 00031 FXOS8700CQ_CTRL_REG2_MODS2(1); // 0b01 gives low noise, low power oversampling mode 00032 00033 // No configuration changes from default 0x00 in CTRL_REG3 00034 // Interrupts will be active low, their outputs in push-pull mode 00035 data[3] = 0x00; 00036 00037 // FXOS8700CQ_CTRL_REG4; 00038 data[4] = 00039 FXOS8700CQ_CTRL_REG4_INT_EN_DRDY; // Enable the Data-Ready interrupt 00040 00041 // No configuration changes from default 0x00 in CTRL_REG5 00042 // Data-Ready interrupt will appear on INT2 00043 data[5] = 0x00; 00044 00045 // Write to the 5 CTRL_REG registers 00046 write_regs(data, 6); 00047 00048 // FXOS8700CQ_XYZ_DATA_CFG 00049 data[0] = FXOS8700CQ_XYZ_DATA_CFG; 00050 data[1] = 00051 FXOS8700CQ_XYZ_DATA_CFG_FS2(1); // 0x01 gives 4g full range, 0.488mg/LSB 00052 write_regs(data, 2); 00053 00054 // Setup for write-through for M_CTRL_REG series 00055 data[0] = FXOS8700CQ_M_CTRL_REG1; 00056 data[1] = 00057 FXOS8700CQ_M_CTRL_REG1_M_ACAL | // set automatic calibration 00058 FXOS8700CQ_M_CTRL_REG1_MO_OS3(7) | // use maximum magnetic oversampling 00059 FXOS8700CQ_M_CTRL_REG1_M_HMS2(3); // enable hybrid sampling (both sensors) 00060 00061 // FXOS8700CQ_M_CTRL_REG2 00062 data[2] = 00063 FXOS8700CQ_M_CTRL_REG2_HYB_AUTOINC_MODE; 00064 00065 // FXOS8700CQ_M_CTRL_REG3 00066 data[3] = 00067 FXOS8700CQ_M_CTRL_REG3_M_ASLP_OS3(7); // maximum sleep magnetic oversampling 00068 00069 // Write to the 3 M_CTRL_REG registers 00070 write_regs(data, 4); 00071 00072 // Peripheral is configured, but disabled 00073 enabled = false; 00074 } 00075 00076 // Destruct class 00077 FXOS8700CQ::~FXOS8700CQ(void) {} 00078 00079 00080 void FXOS8700CQ::enable(void) 00081 { 00082 uint8_t data[2]; 00083 read_regs( FXOS8700CQ_CTRL_REG1, &data[1], 1); 00084 data[1] |= FXOS8700CQ_CTRL_REG1_ACTIVE; 00085 data[0] = FXOS8700CQ_CTRL_REG1; 00086 write_regs(data, 2); // write back 00087 00088 enabled = true; 00089 } 00090 00091 void FXOS8700CQ::disable(void) 00092 { 00093 uint8_t data[2]; 00094 read_regs( FXOS8700CQ_CTRL_REG1, &data[1], 1); 00095 data[0] = FXOS8700CQ_CTRL_REG1; 00096 data[1] &= ~FXOS8700CQ_CTRL_REG1_ACTIVE; 00097 write_regs(data, 2); // write back 00098 00099 enabled = false; 00100 } 00101 00102 00103 uint8_t FXOS8700CQ::status (void) 00104 { 00105 read_regs(FXOS8700CQ_STATUS, &status_reg, 1); 00106 return status_reg; 00107 } 00108 00109 uint8_t FXOS8700CQ::get_whoami (void) 00110 { 00111 uint8_t databyte = 0x00; 00112 read_regs(FXOS8700CQ_WHOAMI, &databyte, 1); 00113 return databyte; 00114 } 00115 00116 uint8_t FXOS8700CQ::get_data (FXOSData_t *data) 00117 { 00118 int i; 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 data->s.mag_x = (raw[0] << 8) | raw[1]; 00128 data->s.mag_y = (raw[2] << 8) | raw[3]; 00129 data->s.mag_z = (raw[4] << 8) | raw[5]; 00130 00131 // Pull out 14-bit, 2's complement, right-justified accelerometer data 00132 data->s.acc_x = (raw[6] << 8) | raw[7]; 00133 data->s.acc_y = (raw[8] << 8) | raw[9]; 00134 data->s.acc_z = (raw[10] << 8) | raw[11]; 00135 00136 // Have to apply corrections to make the int16_t correct 00137 for (i=0; i<3; i++) { 00138 if (data->v[i] > (UINT14_MAX / 2)) { 00139 data->v[i] -= UINT14_MAX; 00140 } 00141 } 00142 00143 return 0; 00144 } 00145 00146 uint8_t FXOS8700CQ::get_accel_scale(void) 00147 { 00148 uint8_t data = 0x00; 00149 read_regs(FXOS8700CQ_XYZ_DATA_CFG, &data, 1); 00150 data &= FXOS8700CQ_XYZ_DATA_CFG_FS2(3); // mask with 0b11 00151 00152 // Choose output value based on masked data 00153 switch(data) { 00154 case FXOS8700CQ_XYZ_DATA_CFG_FS2(0): 00155 return 2; 00156 case FXOS8700CQ_XYZ_DATA_CFG_FS2(1): 00157 return 4; 00158 case FXOS8700CQ_XYZ_DATA_CFG_FS2(2): 00159 return 8; 00160 default: 00161 return 0; 00162 } 00163 } 00164 00165 // Private methods 00166 00167 // Excepting the call to dev_i2c.frequency() in the constructor, 00168 // the use of the mbed I2C class is restricted to these methods 00169 void FXOS8700CQ::read_regs(int reg_addr, uint8_t* data, int len) 00170 { 00171 char t[1] = {reg_addr}; 00172 dev_i2c.write(dev_addr, t, 1, true); 00173 dev_i2c.read(dev_addr, (char *)data, len); 00174 } 00175 00176 void FXOS8700CQ::write_regs(uint8_t* data, int len) 00177 { 00178 dev_i2c.write(dev_addr, (char*)data, len); 00179 }
Generated on Mon Aug 1 2022 17:43:08 by 1.7.2