bool AP_InertialSensor_MPU6000::update( void )
{
int32_t sum[7];
float count_scale;
Vector3f accel_scale = _accel_scale.get();
// wait for at least 1 sample
if (!wait_for_sample(1000)) {
return false;
}
// disable timer procs for mininum time
hal.scheduler->suspend_timer_procs();
/** ATOMIC SECTION w/r/t TIMER PROCESS */
{
for (int i=0; i<7; i++) {
sum[i] = _sum[i];
_sum[i] = 0;
}
_num_samples = _count;
_count = 0;
}
hal.scheduler->resume_timer_procs();
count_scale = 1.0f / _num_samples;
_gyro = Vector3f(_gyro_data_sign[0] * sum[_gyro_data_index[0]],
_gyro_data_sign[1] * sum[_gyro_data_index[1]],
_gyro_data_sign[2] * sum[_gyro_data_index[2]]);
_gyro.rotate(_board_orientation);
_gyro *= _gyro_scale * count_scale;
_gyro -= _gyro_offset;
_accel = Vector3f(_accel_data_sign[0] * sum[_accel_data_index[0]],
_accel_data_sign[1] * sum[_accel_data_index[1]],
_accel_data_sign[2] * sum[_accel_data_index[2]]);
_accel.rotate(_board_orientation);
_accel *= count_scale * MPU6000_ACCEL_SCALE_1G;
_accel.x *= accel_scale.x;
_accel.y *= accel_scale.y;
_accel.z *= accel_scale.z;
_accel -= _accel_offset;
_temp = _temp_to_celsius(sum[_temp_data_index] * count_scale);
if (_last_filter_hz != _mpu6000_filter) {
if (_spi_sem->take(10)) {
_set_filter_register(_mpu6000_filter, 0);
_spi_sem->give();
}
}
return true;
}
bool AP_InertialSensor_MPU9250::update( void )
{
// wait for at least 1 sample
if (!wait_for_sample(1000)) {
return false;
}
_previous_accel[0] = _accel[0];
// disable timer procs for mininum time
hal.scheduler->suspend_timer_procs();
_gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
_accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z);
_num_samples = _sum_count;
_accel_sum.zero();
_gyro_sum.zero();
_sum_count = 0;
hal.scheduler->resume_timer_procs();
_gyro[0].rotate(_board_orientation);
_gyro[0] *= _gyro_scale / _num_samples;
_gyro[0] -= _gyro_offset[0];
_accel[0].rotate(_board_orientation);
_accel[0] *= MPU9250_ACCEL_SCALE_1G / _num_samples;
// rotate for bbone default
_accel[0].rotate(ROTATION_ROLL_180_YAW_90);
_gyro[0].rotate(ROTATION_ROLL_180_YAW_90);
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
// PXF has an additional YAW 180
_accel[0].rotate(ROTATION_YAW_180);
_gyro[0].rotate(ROTATION_YAW_180);
#endif
Vector3f accel_scale = _accel_scale[0].get();
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] -= _accel_offset[0];
if (_last_filter_hz != _mpu6000_filter) {
if (_spi_sem->take(10)) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
_set_filter_register(_mpu6000_filter, 0);
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_error_count = 0;
_spi_sem->give();
}
}
return true;
}
bool AP_InertialSensor_MPU6000_Ext::update( void )
{
// wait for at least 1 sample
if (!wait_for_sample(1000)) {
return false;
}
_previous_accel[0] = _accel[0];
// disable timer procs for mininum time
hal.scheduler->suspend_timer_procs();
_gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
_accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z);
_num_samples = _sum_count;
_accel_sum.zero();
_gyro_sum.zero();
_temp[0] = _temp_sum;
_temp_sum = 0;
_sum_count = 0;
hal.scheduler->resume_timer_procs();
_gyro[0].rotate(_board_orientation);
_gyro[0] *= _gyro_scale / _num_samples;
_gyro[0] -= _gyro_offset[0];
_accel[0].rotate(_board_orientation);
_accel[0] *= MPU6000_ACCEL_SCALE_1G / _num_samples;
Vector3f accel_scale = _accel_scale[0].get();
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] -= _accel_offset[0];
_temp[0] = _temp_to_celsius(_temp[0] / _num_samples);
if (_last_filter_hz != _mpu6000_filter) {
if (_spi_sem->take(10)) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
_set_filter_register(_mpu6000_filter, 0);
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_error_count = 0;
_spi_sem->give();
}
}
return true;
}
/*
process any
*/
bool AP_InertialSensor_MPU6000::update( void )
{
#if !MPU6000_FAST_SAMPLING
if (_sum_count < _sample_count) {
// we don't have enough samples yet
return false;
}
#endif
// we have a full set of samples
uint16_t num_samples;
Vector3f accel, gyro;
hal.scheduler->suspend_timer_procs();
#if MPU6000_FAST_SAMPLING
gyro = _gyro_filtered;
accel = _accel_filtered;
num_samples = 1;
#else
gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
accel(_accel_sum.x, _accel_sum.y, _accel_sum.z);
num_samples = _sum_count;
_accel_sum.zero();
_gyro_sum.zero();
#endif
_sum_count = 0;
hal.scheduler->resume_timer_procs();
gyro *= _gyro_scale / num_samples;
_rotate_and_offset_gyro(_gyro_instance, gyro);
accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
_rotate_and_offset_accel(_accel_instance, accel);
if (_last_filter_hz != _imu.get_filter()) {
if (_spi_sem->take(10)) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
_set_filter_register(_imu.get_filter());
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_spi_sem->give();
}
}
return true;
}
bool AP_InertialSensor_MPU6000_I2C::hardware_init(Sample_rate sample_rate)
{
if (!_i2c_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
}
// Chip reset
uint8_t tries;
uint8_t reg_val;
for (tries = 0; tries<5; tries++) {
hal.i2c->writeRegister(mpu_addr, 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
hal.i2c->writeRegister(mpu_addr, MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO);
hal.scheduler->delay(5);
// check it has woken up
hal.i2c->readRegister(mpu_addr, MPUREG_PWR_MGMT_1, ®_val);
if (reg_val == BIT_PWR_MGMT_1_CLK_ZGYRO) {
break;
}
}
if (tries == 5) {
hal.console->println_P(PSTR("Failed to boot MPU6000 5 times"));
_i2c_sem->give();
return false;
}
// only used for wake-up in accelerometer only low power mode
hal.i2c->writeRegister(mpu_addr, MPUREG_PWR_MGMT_2, 0);
hal.scheduler->delay(1);
uint8_t default_filter, rate;
// 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
_micros_per_sample = 19000;
_delta_time = 0.02;
rate = MPUREG_SMPLRT_50HZ;
default_filter = BITS_DLPF_CFG_10HZ;
break;
case RATE_100HZ:
_micros_per_sample = 9900;
_delta_time = 0.01;
rate = MPUREG_SMPLRT_100HZ;
default_filter = BITS_DLPF_CFG_20HZ;
break;
case RATE_200HZ:
default:
_micros_per_sample = 4900;
_delta_time = 0.005;
rate = MPUREG_SMPLRT_200HZ;
default_filter = BITS_DLPF_CFG_20HZ;
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
hal.i2c->writeRegister(mpu_addr, MPUREG_SMPLRT_DIV, rate);
hal.scheduler->delay(1);
hal.i2c->writeRegister(mpu_addr, MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000?/s
hal.scheduler->delay(1);
// Get chip revision
hal.i2c->readRegister(mpu_addr, MPUREG_PRODUCT_ID, ®_val);
_mpu6000_product_id = reg_val;
// Select Accel scale
if ( (_mpu6000_product_id == MPU6000_REV_A4) || (_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
hal.i2c->writeRegister(mpu_addr, MPUREG_ACCEL_CONFIG, 1<<3);
} else {
// Accel scale 8g (4096 LSB/g)
hal.i2c->writeRegister(mpu_addr, MPUREG_ACCEL_CONFIG, 2<<3);
}
hal.scheduler->delay(1);
#ifndef DISABLE_INTERNAL_MAG
// Enable I2C bypass mode, to work with Magnetometer 5883L
// Disable I2C Master mode
hal.i2c->writeRegister(mpu_addr, MPUREG_USER_CTRL, 0);
hal.i2c->writeRegister(mpu_addr, MPUREG_INT_PIN_CFG, BIT_I2C_BYPASS_EN);
#endif
/* Dump MPU6050 registers
hal.console->println_P(PSTR("MPU6000 registers"));
for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) {
hal.i2c->readRegister(mpu_addr, reg, ®_val);
hal.console->printf_P(PSTR("%02x:%02x "), (unsigned)reg, (unsigned)reg_val);
if ((reg - (MPUREG_PRODUCT_ID-1)) % 16 == 0) {
hal.console->println();
}
}
hal.console->println();*/
_i2c_sem->give();
return true;
}
/*
process any
*/
bool AP_InertialSensor_MPU6000::update( void )
{
#if !MPU6000_FAST_SAMPLING
if (_sum_count < _sample_count) {
// we don't have enough samples yet
return false;
}
#endif
// we have a full set of samples
uint16_t num_samples;
Vector3f accel, gyro;
hal.scheduler->suspend_timer_procs();
#if MPU6000_FAST_SAMPLING
gyro = _gyro_filtered;
accel = _accel_filtered;
num_samples = 1;
#else
gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
accel(_accel_sum.x, _accel_sum.y, _accel_sum.z);
num_samples = _sum_count;
_accel_sum.zero();
_gyro_sum.zero();
#endif
_sum_count = 0;
hal.scheduler->resume_timer_procs();
gyro *= _gyro_scale / num_samples;
accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
accel.rotate(ROTATION_PITCH_180_YAW_90);
gyro.rotate(ROTATION_PITCH_180_YAW_90);
#endif
_publish_accel(_accel_instance, accel);
_publish_gyro(_gyro_instance, gyro);
#if MPU6000_FAST_SAMPLING
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_accel_filter.set_cutoff_frequency(1000, _accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_gyro_filter.set_cutoff_frequency(1000, _gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
#else
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
if (_spi_sem->take(10)) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
_set_filter_register(_accel_filter_cutoff());
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_spi_sem->give();
_last_accel_filter_hz = _accel_filter_cutoff();
}
}
#endif
return true;
}
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;
}
