/**
* Return true if the MPU6000 has new data available for reading.
*
* We use the data ready pin if it is available. Otherwise, read the
* status register.
*/
bool AP_InertialSensor_MPU6000::_data_ready()
{
if (_drdy_pin) {
return _drdy_pin->read() != 0;
}
if (hal.scheduler->in_timerprocess()) {
bool got = _spi_sem->take_nonblocking();
if (got) {
uint8_t status = _register_read(MPUREG_INT_STATUS);
_spi_sem->give();
return (status & BIT_RAW_RDY_INT) != 0;
} else {
return false;
}
} else {
bool got = _spi_sem->take(10);
if (got) {
uint8_t status = _register_read(MPUREG_INT_STATUS);
_spi_sem->give();
return (status & BIT_RAW_RDY_INT) != 0;
} else {
hal.scheduler->panic(
PSTR("PANIC: AP_InertialSensor_MPU6000::_data_ready failed to "
"take SPI semaphore synchronously"));
}
}
return false;
}
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));
}
/*
initialise the sensor
*/
bool AP_InertialSensor_MPU9250::_init_sensor(void)
{
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
_spi_sem = _spi->get_semaphore();
// we need to suspend timers to prevent other SPI drivers grabbing
// the bus while we do the long initialisation
hal.scheduler->suspend_timer_procs();
uint8_t whoami = _register_read(MPUREG_WHOAMI);
if (whoami != 0x71) {
// TODO: we should probably accept multiple chip
// revisions. This is the one on the PXF
hal.console->printf("MPU9250: unexpected WHOAMI 0x%x\n", (unsigned)whoami);
return false;
}
uint8_t tries = 0;
do {
bool success = _hardware_init();
if (success) {
hal.scheduler->delay(10);
if (!_spi_sem->take(100)) {
hal.console->printf("MPU9250: Unable to get semaphore");
return false;
}
uint8_t status = _register_read(MPUREG_INT_STATUS);
if ((status & BIT_RAW_RDY_INT) != 0) {
_spi_sem->give();
break;
}
_spi_sem->give();
}
if (tries++ > 5) {
return false;
}
} while (1);
hal.scheduler->resume_timer_procs();
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
_product_id = AP_PRODUCT_ID_MPU9250;
// start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9250::_poll_data));
#if MPU9250_DEBUG
_dump_registers();
#endif
return true;
}
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"));
}
void AP_InertialSensor_LSM303D::_read_data_transaction_mag() {
if (_register_read(ADDR_CTRL_REG7) != _reg7_expected) {
hal.console->println_P(
PSTR("LSM303D _read_data_transaction_accel: _reg7_expected unexpected"));
// reset();
return;
}
struct {
uint8_t cmd;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} raw_mag_report;
/* fetch data from the sensor */
memset(&raw_mag_report, 0, sizeof(raw_mag_report));
raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
_spi->transaction((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
_mag_sum.x = raw_mag_report.x;
_mag_sum.y = raw_mag_report.y;
_mag_sum.z = raw_mag_report.z;
}
/**
* Return true if the MPU6000 has new data available for reading.
*
* We use the data ready pin if it is available. Otherwise, read the
* status register.
*/
bool AP_InertialSensor_MPU6000::_data_ready()
{
if (_drdy_pin) {
return _drdy_pin->read() != 0;
}
uint8_t status = _register_read(MPUREG_INT_STATUS);
return (status & BIT_RAW_RDY_INT) != 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
}
// dump all config registers - used for debug
void AP_InertialSensor_MPU6000::_dump_registers(void)
{
hal.console->println_P(PSTR("MPU6000 registers"));
for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) {
uint8_t v = _register_read(reg);
hal.console->printf_P(PSTR("%02x:%02x "), (unsigned)reg, (unsigned)v);
if ((reg - (MPUREG_PRODUCT_ID-1)) % 16 == 0) {
hal.console->println();
}
}
hal.console->println();
}
// dump all config registers - used for debug
void AP_InertialSensor_L3GD20::_dump_registers(void)
{
hal.console->println_P(PSTR("L3GD20 registers"));
if (_spi_sem->take(100)) {
for (uint8_t reg=ADDR_WHO_AM_I; reg<=56; reg++) { // 0x38 = 56
uint8_t v = _register_read(reg);
hal.console->printf_P(PSTR("%02x:%02x "), (unsigned)reg, (unsigned)v);
if ((reg - (ADDR_WHO_AM_I-1)) % 16 == 0) {
hal.console->println();
}
}
hal.console->println();
_spi_sem->give();
}
}
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;
}
uint16_t AP_InertialSensor_L3GD20::_init_sensor( Sample_rate sample_rate )
{
if (_initialised) return _L3GD20_product_id;
_initialised = true;
_spi = hal.spi->device(AP_HAL::SPIDevice_L3GD20);
_spi_sem = _spi->get_semaphore();
#ifdef L3GD20_DRDY_PIN
_drdy_pin = hal.gpio->channel(L3GD20_DRDY_PIN);
_drdy_pin->mode(HAL_GPIO_INPUT);
#endif
hal.scheduler->suspend_timer_procs();
// Test WHOAMI
uint8_t whoami = _register_read(ADDR_WHO_AM_I);
if (whoami != WHO_I_AM) {
// TODO: we should probably accept multiple chip
// revisions. This is the one on the PXF
hal.console->printf("L3GD20: unexpected WHOAMI 0x%x\n", (unsigned)whoami);
hal.scheduler->panic(PSTR("L3GD20: bad WHOAMI"));
}
uint8_t tries = 0;
do {
bool success = _hardware_init(sample_rate);
if (success) {
hal.scheduler->delay(5+2);
if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("L3GD20: Unable to get semaphore"));
}
if (_data_ready()) {
_spi_sem->give();
break;
} else {
hal.console->println_P(
PSTR("L3GD20 startup failed: no data ready"));
}
_spi_sem->give();
}
if (tries++ > 5) {
hal.scheduler->panic(PSTR("PANIC: failed to boot L3GD20 5 times"));
}
} while (1);
hal.scheduler->resume_timer_procs();
/* read the first lot of data.
* _read_data_transaction requires the spi semaphore to be taken by
* its caller. */
_last_sample_time_micros = hal.scheduler->micros();
hal.scheduler->delay(10);
if (_spi_sem->take(100)) {
_read_data_transaction();
_spi_sem->give();
}
// start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_L3GD20::_poll_data));
#if L3GD20_DEBUG
_dump_registers();
#endif
return _L3GD20_product_id;
}
/*
initialise the sensor
*/
uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
{
if (_initialised) return _mpu9250_product_id;
_initialised = true;
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
_spi_sem = _spi->get_semaphore();
// we need to suspend timers to prevent other SPI drivers grabbing
// the bus while we do the long initialisation
hal.scheduler->suspend_timer_procs();
uint8_t whoami = _register_read(MPUREG_WHOAMI);
if (whoami != 0x71) {
// TODO: we should probably accept multiple chip
// revisions. This is the one on the PXF
hal.console->printf("MPU9250: unexpected WHOAMI 0x%x\n", (unsigned)whoami);
hal.scheduler->panic("MPU9250: bad WHOAMI");
}
uint8_t tries = 0;
do {
bool success = _hardware_init(sample_rate);
if (success) {
hal.scheduler->delay(10);
if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore"));
}
uint8_t status = _register_read(MPUREG_INT_STATUS);
if ((status & BIT_RAW_RDY_INT) != 0) {
_spi_sem->give();
break;
} else {
hal.console->println_P(
PSTR("MPU9250 startup failed: no data ready"));
}
_spi_sem->give();
}
if (tries++ > 5) {
hal.scheduler->panic(PSTR("PANIC: failed to boot MPU9250 5 times"));
}
} while (1);
hal.scheduler->resume_timer_procs();
/* read the first lot of data.
* _read_data_transaction requires the spi semaphore to be taken by
* its caller. */
hal.scheduler->delay(10);
if (_spi_sem->take(100)) {
_read_data_transaction();
_spi_sem->give();
}
// start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9250::_poll_data));
#if MPU9250_DEBUG
_dump_registers();
#endif
return _mpu9250_product_id;
}
bool AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
{
if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
}
// Chip reset
uint8_t tries;
for (tries = 0; tries<5; tries++) {
register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
hal.scheduler->delay(100);
// Wake up device and select GyroZ clock. Note that the
// MPU6000 starts up in sleep mode, and it can take some time
// for it to come out of sleep
register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO);
hal.scheduler->delay(5);
// check it has woken up
if (_register_read(MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) {
break;
}
#if MPU6000_DEBUG
_dump_registers();
#endif
}
if (tries == 5) {
hal.console->println_P(PSTR("Failed to boot MPU6000 5 times"));
_spi_sem->give();
return false;
}
register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode
hal.scheduler->delay(1);
// Disable I2C bus (recommended on datasheet)
register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
hal.scheduler->delay(1);
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(_mpu6000_filter, default_filter);
// set sample rate to 200Hz, and use _sample_divider to give
// the requested rate to the application
register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ);
hal.scheduler->delay(1);
register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s
hal.scheduler->delay(1);
// read the product ID rev c has 1/2 the sensitivity of rev d
_mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
//Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
(_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) {
// Accel scale 8g (4096 LSB/g)
// Rev C has different scaling than rev D
register_write(MPUREG_ACCEL_CONFIG,1<<3);
} else {
// Accel scale 8g (4096 LSB/g)
register_write(MPUREG_ACCEL_CONFIG,2<<3);
}
hal.scheduler->delay(1);
// configure interrupt to fire when new data arrives
register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);
hal.scheduler->delay(1);
// clear interrupt on any read, and hold the data ready pin high
// until we clear the interrupt
register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN);
hal.scheduler->delay(1);
_spi_sem->give();
return true;
}
