void AP_InertialSensor_LSM303D::disable_i2c(void)
{
uint8_t a = _register_read(0x02);
_register_write(0x02, (0x10 | a));
a = _register_read(0x02);
_register_write(0x02, (0xF7 & a));
a = _register_read(0x15);
_register_write(0x15, (0x80 | a));
a = _register_read(0x02);
_register_write(0x02, (0xE7 & a));
}
uint8_t AP_InertialSensor_L3GD20::set_range(uint8_t max_dps)
{
uint8_t bits = REG4_BDU;
float new_range_scale_dps_digit;
float new_range;
if (max_dps == 0) {
max_dps = 2000;
}
if (max_dps <= 250) {
new_range = 250;
bits |= RANGE_250DPS;
new_range_scale_dps_digit = 8.75e-3f;
} else if (max_dps <= 500) {
new_range = 500;
bits |= RANGE_500DPS;
new_range_scale_dps_digit = 17.5e-3f;
} else if (max_dps <= 2000) {
new_range = 2000;
bits |= RANGE_2000DPS;
new_range_scale_dps_digit = 70e-3f;
} else {
return -1;
}
// _gyro_range_rad_s = new_range / 180.0f * M_PI_F;
// _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
_gyro_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
_register_write(ADDR_CTRL_REG4, bits);
return 0;
}
uint8_t AP_InertialSensor_L3GD20::set_samplerate(uint16_t frequency)
{
uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
if (frequency == 0)
frequency = 760;
/* use limits good for H or non-H models */
if (frequency <= 100) {
// _current_rate = 95;
bits |= RATE_95HZ_LP_25HZ;
} else if (frequency <= 200) {
// _current_rate = 190;
bits |= RATE_190HZ_LP_50HZ;
} else if (frequency <= 400) {
// _current_rate = 380;
bits |= RATE_380HZ_LP_50HZ;
} else if (frequency <= 800) {
// _current_rate = 760;
bits |= RATE_760HZ_LP_50HZ;
} else {
return -1;
}
_register_write(ADDR_CTRL_REG1, bits);
return 0;
}
void AP_InertialSensor_LSM303D::_register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val;
val = _register_read(reg);
val &= ~clearbits;
val |= setbits;
_register_write(reg, val);
}
/*
useful when debugging SPI bus errors
*/
void AP_InertialSensor_L3GD20::_register_write_check(uint8_t reg, uint8_t val)
{
uint8_t readed;
_register_write(reg, val);
readed = _register_read(reg);
if (readed != val){
hal.console->printf_P(PSTR("Values doesn't match; written: %02x; read: %02x "), val, readed);
}
#if L3GD20_DEBUG
hal.console->printf_P(PSTR("Values written: %02x; readed: %02x "), val, readed);
#endif
}
void AP_InertialSensor_L3GD20::disable_i2c(void)
{
uint8_t retries = 10;
while (retries--) {
// add retries
uint8_t a = _register_read(0x05);
_register_write(0x05, (0x20 | a));
if (_register_read(0x05) == (a | 0x20)) {
return;
}
}
hal.scheduler->panic(PSTR("L3GD20: Unable to disable I2C"));
}
bool AP_Compass_AK8963::_calibrate()
{
error("CALIBRATTION START\n");
_register_write(AK8963_CNTL1, AK8963_FUSE_MODE | _magnetometer_adc_resolution); /* Enable FUSE-mode in order to be able to read calibreation data */
hal.scheduler->delay(10);
uint8_t response[3];
_register_read(AK8963_ASAX, 0x03, response);
for (int i = 0; i < 3; i++) {
float data = response[i];
magnetometer_ASA[i] = ((data-128)/256+1);
error("%d: %lf\n", i, magnetometer_ASA[i]);
}
error("CALIBRATTION END\n");
return true;
}
bool AP_Compass_AK8963::init()
{
_healthy[0] = true;
_field[0].x = 0.0f;
_field[0].y = 0.0f;
_field[0].z = 0.0f;
hal.scheduler->suspend_timer_procs();
if (!_backend->sem_take_blocking()) {
error("_spi_sem->take failed\n");
return false;
}
if (!_backend_init()) {
_backend->sem_give();
return false;
}
_register_write(AK8963_CNTL2, AK8963_RESET); /* Reset AK8963 */
hal.scheduler->delay(1000);
int id_mismatch_count;
uint8_t deviceid;
for (id_mismatch_count = 0; id_mismatch_count < 5; id_mismatch_count++) {
_register_read(AK8963_WIA, 0x01, &deviceid); /* Read AK8963's id */
if (deviceid == AK8963_Device_ID) {
break;
}
error("trying to read AK8963's ID once more...\n");
_backend_reset();
hal.scheduler->delay(100);
_dump_registers();
}
if (id_mismatch_count == 5) {
_initialised = false;
hal.console->printf("WRONG AK8963 DEVICE ID: 0x%x\n", (unsigned)deviceid);
hal.scheduler->panic(PSTR("AK8963: bad DEVICE ID"));
}
_calibrate();
_initialised = true;
#if AK8963_SELFTEST
if (_self_test()) {
_initialised = true;
} else {
_initialised = false;
}
#endif
/* Register value to continuous measurement */
_register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | _magnetometer_adc_resolution);
_backend->sem_give();
hal.scheduler->resume_timer_procs();
hal.scheduler->register_timer_process( AP_HAL_MEMBERPROC(&AP_Compass_AK8963::_update));
_start_conversion();
_initialised = true;
return _initialised;
}
bool AP_Compass_AK8963::_self_test()
{
bool success = false;
/* see AK8963.pdf p.19 */
/* Set power-down mode */
_register_write(AK8963_CNTL1, AK8963_POWERDOWN_MODE | _magnetometer_adc_resolution);
/* Turn the internal magnetic field on */
_register_write(AK8963_ASTC, AK8983_SELFTEST_MAGNETIC_FIELD_ON);
/* Register value to self-test mode in 14-bit */
_register_write(AK8963_CNTL1, AK8963_SELFTEST_MODE | _magnetometer_adc_resolution);
_start_conversion();
hal.scheduler->delay(20);
_read_raw();
float hx = _mag_x;
float hy = _mag_y;
float hz = _mag_z;
error("AK8963's SELF-TEST STARTED\n");
switch (_magnetometer_adc_resolution) {
bool hx_is_in_range;
bool hy_is_in_range;
bool hz_is_in_range;
case AK8963_14BIT_ADC:
hx_is_in_range = (hx >= - 50) && (hx <= 50);
hy_is_in_range = (hy >= - 50) && (hy <= 50);
hz_is_in_range = (hz >= - 800) && (hz <= -200);
if (hx_is_in_range && hy_is_in_range && hz_is_in_range) {
success = true;
}
break;
case AK8963_16BIT_ADC:
hx_is_in_range = (hx >= -200) && (hx <= 200);
hy_is_in_range = (hy >= -200) && (hy <= 200);
hz_is_in_range = (hz >= -3200) && (hz <= -800);
if (hx_is_in_range && hy_is_in_range && hz_is_in_range) {
success = true;
}
break;
default:
success = false;
hal.scheduler->panic(PSTR("Wrong AK8963's ADC resolution selected"));
break;
}
error("AK8963's SELF-TEST ENDED: %f %f %f\n", hx, hy, hz);
/* Turn the internal magnetic field off */
_register_write(AK8963_ASTC, 0x0);
/* Register value to continuous measurement in 14-bit */
_register_write(AK8963_CNTL1, AK8963_POWERDOWN_MODE | _magnetometer_adc_resolution);
return success;
}
bool AP_InertialSensor_L3GD20::_hardware_init(Sample_rate sample_rate)
{
if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("L3GD20: 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();
// Chip reset
/* set default configuration */
_register_write(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
hal.scheduler->delay(1);
_register_write(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
hal.scheduler->delay(1);
_register_write(ADDR_CTRL_REG3, 0x08); /* DRDY enable */
hal.scheduler->delay(1);
_register_write(ADDR_CTRL_REG4, REG4_BDU);
hal.scheduler->delay(1);
_register_write(ADDR_CTRL_REG5, 0);
hal.scheduler->delay(1);
_register_write(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
hal.scheduler->delay(1);
/* disable FIFO. This makes things simpler and ensures we
* aren't getting stale data. It means we must run the hrt
* callback fast enough to not miss data. */
_register_write(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
hal.scheduler->delay(1);
set_samplerate(0); // 760Hz
hal.scheduler->delay(1);
set_range(L3GD20_DEFAULT_RANGE_DPS);
hal.scheduler->delay(1);
// //TODO: Filtering
// 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(_L3GD20_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;
}
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;
}
