int MPU9250::reset_mpu()
{
uint8_t retries;
switch (_whoami) {
case MPU_WHOAMI_9250:
case MPU_WHOAMI_6500:
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO);
write_checked_reg(MPUREG_PWR_MGMT_2, 0);
usleep(1000);
break;
}
// Enable I2C bus or Disable I2C bus (recommended on data sheet)
write_checked_reg(MPUREG_USER_CTRL, is_i2c() ? 0 : BIT_I2C_IF_DIS);
// SAMPLE RATE
_set_sample_rate(_sample_rate);
_set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ);
// Gyro scale 2000 deg/s ()
switch (_whoami) {
case MPU_WHOAMI_9250:
case MPU_WHOAMI_6500:
write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
break;
}
// correct gyro scale factors
// scale to rad/s in SI units
// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
// scaling factor:
// 1/(2^15)*(2000/180)*PI
_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
set_accel_range(ACCEL_RANGE_G);
// INT CFG => Interrupt on Data Ready
write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready
#ifdef USE_I2C
bool bypass = !_mag->is_passthrough();
#else
bool bypass = false;
#endif
/* INT: Clear on any read.
* If this instance is for a device is on I2C bus the Mag will have an i2c interface
* that it will use to access the either: a) the internal mag device on the internal I2C bus
* or b) it could be used to access a downstream I2C devices connected to the chip on
* it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master
* controller that chip provides as a SPI to I2C bridge.
* so bypass is true if the mag has an i2c non null interfaces.
*/
write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0));
write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ);
retries = 3;
bool all_ok = false;
while (!all_ok && retries--) {
// Assume all checked values are as expected
all_ok = true;
uint8_t reg;
uint8_t bankcheck = 0;
for (uint8_t i = 0; i < _num_checked_registers; i++) {
if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) {
write_reg(_checked_registers[i], _checked_values[i]);
PX4_ERR("Reg %d is:%d s/b:%d Tries:%d - bank s/b %d, is %d", _checked_registers[i], reg, _checked_values[i], retries,
REG_BANK(_checked_registers[i]), bankcheck);
all_ok = false;
}
}
}
return all_ok ? OK : -EIO;
}
int
MPU9250::init()
{
#if defined(USE_I2C)
unsigned dummy;
use_i2c(_interface->ioctl(MPUIOCGIS_I2C, dummy));
#endif
/*
* If the MPU is using I2C we should reduce the sample rate to 200Hz and
* make the integration autoreset faster so that we integrate just one
* sample since the sampling rate is already low.
*/
if (is_i2c()) {
_sample_rate = 200;
_accel_int.set_autoreset_interval(1000000 / 1000);
_gyro_int.set_autoreset_interval(1000000 / 1000);
}
int ret = probe();
if (ret != OK) {
DEVICE_DEBUG("MPU9250 probe failed");
return ret;
}
/* do init */
ret = CDev::init();
/* if init failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
return ret;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
ret = -ENOMEM;
if (_accel_reports == nullptr) {
return ret;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr) {
return ret;
}
if (reset_mpu() != OK) {
PX4_ERR("Exiting! Device failed to take initialization");
return ret;
}
/* Initialize offsets and scales */
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
// set software low pass filter for controllers
param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF");
float accel_cut = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
if (accel_cut_ph != PARAM_INVALID && (param_get(accel_cut_ph, &accel_cut) == PX4_OK)) {
PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut));
_accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
_accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
_accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
} else {
PX4_ERR("IMU_ACCEL_CUTOFF param invalid");
}
param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
float gyro_cut = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) {
PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut));
_gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
_gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
_gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
} else {
PX4_ERR("IMU_GYRO_CUTOFF param invalid");
}
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("gyro init failed");
return ret;
}
#ifdef USE_I2C
if (!_mag->is_passthrough() && _mag->_interface->init() != PX4_OK) {
PX4_ERR("failed to setup ak8963 interface");
}
#endif /* USE_I2C */
/* do CDev init for the mag device node, keep it optional */
if (_whoami == MPU_WHOAMI_9250) {
ret = _mag->init();
}
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("mag init failed");
return ret;
}
if (_whoami == MPU_WHOAMI_9250) {
ret = _mag->ak8963_reset();
}
if (ret != OK) {
DEVICE_DEBUG("mag reset failed");
return ret;
}
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_accel_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
return ret;
}
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_gyro->_gyro_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
return ret;
}
return ret;
}
int
MPU9250::init()
{
irqstate_t state;
#if defined(USE_I2C)
use_i2c(_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C);
#endif
/*
* If the MPU is using I2C we should reduce the sample rate to 200Hz and
* make the integration autoreset faster so that we integrate just one
* sample since the sampling rate is already low.
*/
if (is_i2c() && !_magnetometer_only) {
_sample_rate = 200;
_accel_int.set_autoreset_interval(1000000 / 1000);
_gyro_int.set_autoreset_interval(1000000 / 1000);
}
int ret = probe();
if (ret != OK) {
PX4_DEBUG("MPU9250 probe failed");
return ret;
}
state = px4_enter_critical_section();
_reset_wait = hrt_absolute_time() + 100000;
px4_leave_critical_section(state);
if (reset_mpu() != OK) {
PX4_ERR("Exiting! Device failed to take initialization");
return ret;
}
if (!_magnetometer_only) {
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s));
ret = -ENOMEM;
if (_accel_reports == nullptr) {
return ret;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s));
if (_gyro_reports == nullptr) {
return ret;
}
/* Initialize offsets and scales */
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
// set software low pass filter for controllers
param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF");
float accel_cut = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
if (accel_cut_ph != PARAM_INVALID && (param_get(accel_cut_ph, &accel_cut) == PX4_OK)) {
PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut));
_accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
_accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
_accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
}
param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
float gyro_cut = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) {
PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut));
_gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
_gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
_gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
}
/* do CDev init for the accel device node */
ret = _accel->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
PX4_DEBUG("accel init failed");
return ret;
}
/* do CDev init for the gyro device node */
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
PX4_DEBUG("gyro init failed");
return ret;
}
}
/* Magnetometer setup */
if (_whoami == MPU_WHOAMI_9250) {
#ifdef USE_I2C
up_udelay(100);
if (!_mag->is_passthrough() && _mag->_interface->init() != PX4_OK) {
PX4_ERR("failed to setup ak8963 interface");
}
#endif /* USE_I2C */
/* do CDev init for the mag device node */
ret = _mag->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
PX4_DEBUG("mag init failed");
return ret;
}
ret = _mag->ak8963_reset();
if (ret != OK) {
PX4_DEBUG("mag reset failed");
return ret;
}
}
measure();
if (!_magnetometer_only) {
/* advertise sensor topic, measure manually to initialize valid report */
sensor_accel_s arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel->_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_accel_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
return ret;
}
/* advertise sensor topic, measure manually to initialize valid report */
sensor_gyro_s grp;
_gyro_reports->get(&grp);
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_gyro->_gyro_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
return ret;
}
}
return ret;
}
int MPU9250::reset()
{
irqstate_t state;
// Hold off sampling for 4 ms
state = px4_enter_critical_section();
_reset_wait = hrt_absolute_time() + 10000;
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO);
write_checked_reg(MPUREG_PWR_MGMT_2, 0);
px4_leave_critical_section(state);
usleep(1000);
// Enable I2C bus or Disable I2C bus (recommended on data sheet)
write_checked_reg(MPUREG_USER_CTRL, is_i2c() ? 0 : BIT_I2C_IF_DIS);
// SAMPLE RATE
_set_sample_rate(_sample_rate);
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
// was 90 Hz, but this ruins quality and does not improve the
// system response
_set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ);
// Gyro scale 2000 deg/s ()
write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
// correct gyro scale factors
// scale to rad/s in SI units
// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
// scaling factor:
// 1/(2^15)*(2000/180)*PI
_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
set_accel_range(ACCEL_RANGE_G);
// INT CFG => Interrupt on Data Ready
write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready
#ifdef USE_I2C
bool bypass = !_mag->is_passthrough();
#else
bool bypass = false;
#endif
/* INT: Clear on any read.
* If this instance is for a device is on I2C bus the Mag will have an i2c interface
* that it will use to access the either: a) the internal mag device on the internal I2C bus
* or b) it could be used to access a downstream I2C devices connected to the chip on
* it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master
* controller that chip provides as a SPI to I2C bridge.
* so bypass is true if the mag has an i2c non null interfaces.
*/
write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0));
write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ);
uint8_t retries = 3;
bool all_ok = false;
while (!all_ok && retries--) {
// Assume all checked values are as expected
all_ok = true;
uint8_t reg;
for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) {
if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) {
write_reg(_checked_registers[i], _checked_values[i]);
PX4_ERR("Reg %d is:%d s/b:%d Tries:%d", _checked_registers[i], reg, _checked_values[i], retries);
all_ok = false;
}
}
}
return all_ok ? OK : -EIO;
}
