int MPU9250::reset() { irqstate_t state; /* When the mpu9250 starts from 0V the internal power on circuit * per the data sheet will require: * * Start-up time for register read/write From power-up Typ:11 max:100 ms * */ px4_usleep(110000); // Hold off sampling until done (100 MS will be shortened) state = px4_enter_critical_section(); _reset_wait = hrt_absolute_time() + 100000; px4_leave_critical_section(state); int ret; ret = reset_mpu(); if (ret == OK && (_whoami == MPU_WHOAMI_9250)) { ret = _mag->ak8963_reset(); } state = px4_enter_critical_section(); _reset_wait = hrt_absolute_time() + 10; px4_leave_critical_section(state); 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::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; }