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::init()
{
#if defined(USE_I2C)
unsigned dummy;
use_i2c(_interface->ioctl(MPUIOCGIS_I2C, dummy));
#endif
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));
if (_accel_reports == nullptr) {
goto out;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr) {
goto out;
}
if (reset() != OK) {
goto out;
}
/* 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;
/* 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() != OK) {
warnx("failed to setup ak8963 interface");
}
#endif
/* do CDev init for the mag device node, keep it optional */
ret = _mag->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("mag init 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) {
warnx("ADVERT FAIL");
}
/* 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) {
warnx("ADVERT FAIL");
}
out:
return ret;
}
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;
}
