void LSM303D::reset() { // ensure the chip doesn't interpret any other bus traffic as I2C disable_i2c(); /* enable accel*/ _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; write_reg(ADDR_CTRL_REG1, _reg1_expected); /* enable mag */ _reg7_expected = REG7_CONT_MODE_M; write_reg(ADDR_CTRL_REG7, _reg7_expected); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_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(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); _accel_read = 0; _mag_read = 0; }
void LSM303D::reset() { /* enable accel*/ write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); /* enable mag */ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); _accel_read = 0; _mag_read = 0; }
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; }
bool AP_InertialSensor_LSM303D::_hardware_init(Sample_rate sample_rate) { if (!_spi_sem->take(100)) { hal.scheduler->panic(PSTR("LSM303D: Unable to get semaphore")); } // initially run the bus at low speed _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); // ensure the chip doesn't interpret any other bus traffic as I2C disable_i2c(); /* enable accel*/ _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; _register_write(ADDR_CTRL_REG1, _reg1_expected); /* enable mag */ _reg7_expected = REG7_CONT_MODE_M; _register_write(ADDR_CTRL_REG7, _reg7_expected); _register_write(ADDR_CTRL_REG5, REG5_RES_HIGH_M); _register_write(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 _register_write(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); // Hardware filtering // 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(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); // TODO: Software filtering // accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); // uint8_t default_filter; // // sample rate and filtering // // to minimise the effects of aliasing we choose a filter // // that is less than half of the sample rate // switch (sample_rate) { // case RATE_50HZ: // // this is used for plane and rover, where noise resistance is // // more important than update rate. Tests on an aerobatic plane // // show that 10Hz is fine, and makes it very noise resistant // default_filter = BITS_DLPF_CFG_10HZ; // _sample_shift = 2; // break; // case RATE_100HZ: // default_filter = BITS_DLPF_CFG_20HZ; // _sample_shift = 1; // break; // case RATE_200HZ: // default: // default_filter = BITS_DLPF_CFG_20HZ; // _sample_shift = 0; // break; // } // _set_filter_register(_LSM303D_filter, default_filter); // now that we have initialised, we set the SPI bus speed to high _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); _spi_sem->give(); return true; }