int
FXOS8700CQ::accel_set_samplerate(unsigned frequency)
{
uint8_t setbits = 0;
/* The selected ODR is reduced by a factor of two when the device is operated in hybrid mode.*/
uint8_t active = read_reg(FXOS8700CQ_CTRL_REG1) & CTRL_REG1_ACTIVE;
if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) {
frequency = FXOS8700C_ACCEL_DEFAULT_RATE;
}
if (frequency <= 25) {
setbits = CTRL_REG1_DR(4); // Use 50 as it is 50 / 2
_accel_samplerate = 25;
} else if (frequency <= 50) {
setbits = CTRL_REG1_DR(3); // Use 100 as it is 100 / 2
_accel_samplerate = 50;
} else if (frequency <= 100) {
setbits = CTRL_REG1_DR(2); // Use 200 as it is 200 / 2
_accel_samplerate = 100;
} else if (frequency <= 200) {
setbits = CTRL_REG1_DR(1); // Use 400 as it is 400 / 2;
_accel_samplerate = 200;
} else if (frequency <= 400) {
setbits = CTRL_REG1_DR(0); // Use 800 as it is 800 / 2;
_accel_samplerate = 400;
} else {
return -EINVAL;
}
modify_reg(FXOS8700CQ_CTRL_REG1, CTRL_REG1_ACTIVE, 0);
modify_reg(FXOS8700CQ_CTRL_REG1, CTRL_REG1_DR_MASK, setbits);
modify_reg(FXOS8700CQ_CTRL_REG1, 0, active);
return OK;
}
void MMA8653::setup() {
// Set up the I2C peripheral
Wire.begin();
// Reset the device, to put it into a known state.
Wire.beginTransmission(MMA8653_ADDRESS);
Wire.write(CTRL_REG2);
Wire.write(CTRL_REG2_RST);
Wire.endTransmission();
delay(1); // Allow the device to reset
// Check that we're talking to the right kind of device
Wire.beginTransmission(MMA8653_ADDRESS);
Wire.write(WHO_AM_I);
Wire.endTransmission(false);
Wire.requestFrom(MMA8653_ADDRESS, 1);
while(Wire.available()) {
Wire.read();
// TODO: Test if this is equal to 0x5A?
}
// Configure for 8G sensitivity
Wire.beginTransmission(MMA8653_ADDRESS);
Wire.write(XYZ_DATA_CFG);
Wire.write(XYZ_DATA_CFG_8G);
Wire.endTransmission();
// Enable data ready interrupt on interrput pin 1
Wire.beginTransmission(MMA8653_ADDRESS);
Wire.write(CTRL_REG4);
Wire.write(CTRL_REG4_INT_EN_DRDY);
Wire.endTransmission();
Wire.beginTransmission(MMA8653_ADDRESS);
Wire.write(CTRL_REG5);
Wire.write(CTRL_REG5_INT_CFG_DRDY);
Wire.endTransmission();
// Put in fast-read mode, with 800Hz output rate, and activate
Wire.beginTransmission(MMA8653_ADDRESS);
Wire.write(CTRL_REG1);
Wire.write(CTRL_REG1_ACTIVE | CTRL_REG1_F_READ | CTRL_REG1_DR(0));
Wire.endTransmission();
}
void
FXOS8700CQ::reset()
{
/* enable accel set it To Standby */
write_checked_reg(FXOS8700CQ_CTRL_REG1, 0);
write_checked_reg(FXOS8700CQ_XYZ_DATA_CFG, 0);
/* Use hybird mode to read Accel and Mag */
write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_AM | M_CTRL_REG1_OS(7));
/* Use the hybird auto increment mode to read all the data at the same time */
write_checked_reg(FXOS8700CQ_M_CTRL_REG2, CTRL_REG2_AUTO_INC);
accel_set_range(FXOS8700C_ACCEL_DEFAULT_RANGE_G);
accel_set_samplerate(FXOS8700C_ACCEL_DEFAULT_RATE);
accel_set_driver_lowpass_filter((float)FXOS8700C_ACCEL_DEFAULT_RATE, (float)FXOS8700C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
// we setup the anti-alias on-chip filter as 50Hz. We believe
// this operates in the analog domain, and is critical for
// anti-aliasing. The 2 pole software filter is designed to
// operate in conjunction with this on-chip filter
accel_set_onchip_lowpass_filter_bandwidth(FXOS8700C_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
mag_set_range(FXOS8700C_MAG_DEFAULT_RANGE_GA);
/* enable set it To Standby mode at 800 Hz which becomes 400 Hz due to hybird mode */
write_checked_reg(FXOS8700CQ_CTRL_REG1, CTRL_REG1_DR(0) | CTRL_REG1_ACTIVE);
_accel_read = 0;
_mag_read = 0;
}
