Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: fixed handling of user_ctrl register

Browse Source 
with aux bus implementations
This commit is contained in:
hiro2233 2016-11-23 06:02:39 +00:00 committed by Andrew Tridgell
parent 29bbee421e
commit cb7f46d653
4 changed files with 37 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -321,29 +321,18 @@ bool AP_InertialSensor_MPU6000::_init()
void AP_InertialSensor_MPU6000::_fifo_reset()
{
uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0;
uint8_t user_ctrl = _last_stat_user_ctrl;
user_ctrl &= ~(BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN);
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
_register_write(MPUREG_FIFO_EN, 0);
_register_write(MPUREG_USER_CTRL, user_ctrl);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN);
_register_write(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN);
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN, true);
hal.scheduler->delay_microseconds(1);
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
}
void AP_InertialSensor_MPU6000::_fifo_enable()
{
uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0;
_register_write(MPUREG_FIFO_EN, 0);
hal.scheduler->delay_microseconds(1);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN);
hal.scheduler->delay_microseconds(1);
_register_write(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN,
true);
hal.scheduler->delay(1);
_last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN;
}
bool AP_InertialSensor_MPU6000::_has_auxiliary_bus()
@ -365,7 +354,7 @@ void AP_InertialSensor_MPU6000::start()
hal.scheduler->delay(1);
// always use FIFO
_fifo_enable();
_fifo_reset();
// grab the used instances
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU6000));
@ -786,17 +775,18 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
_dev->get_semaphore()->give();
return false;
}
// Chip reset
uint8_t tries;
for (tries = 0; tries < 5; tries++) {
uint8_t user_ctrl = _register_read(MPUREG_USER_CTRL);
_last_stat_user_ctrl = _register_read(MPUREG_USER_CTRL);
/* First disable the master I2C to avoid hanging the slaves on the
* aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus
* is used */
if (user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
_register_write(MPUREG_USER_CTRL, user_ctrl & ~BIT_USER_CTRL_I2C_MST_EN);
if (_last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
_last_stat_user_ctrl &= ~BIT_USER_CTRL_I2C_MST_EN;
_register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl);
hal.scheduler->delay(10);
}
@ -808,7 +798,8 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
/* Disable I2C bus if SPI selected (Recommended in Datasheet to be
* done just after the device is reset) */
_register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
_last_stat_user_ctrl |= BIT_USER_CTRL_I2C_IF_DIS;
_register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl);
}
// Wake up device and select GyroZ clock. Note that the
@ -971,11 +962,11 @@ void AP_MPU6000_AuxiliaryBus::_configure_slaves()
auto &backend = AP_InertialSensor_MPU6000::from(_ins_backend);
/* Enable the I2C master to slaves on the auxiliary I2C bus*/
uint8_t user_ctrl = backend._register_read(MPUREG_USER_CTRL);
backend._register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN);
if (!(backend._last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN)) {
backend._last_stat_user_ctrl |= BIT_USER_CTRL_I2C_MST_EN;
backend._register_write(MPUREG_USER_CTRL, backend._last_stat_user_ctrl);
}
backend._master_i2c_enable = true;
/* stop condition between reads; clock at 400kHz */
backend._register_write(MPUREG_I2C_MST_CTRL,
BIT_I2C_MST_P_NSR | BIT_I2C_MST_CLK_400KHZ);

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -61,7 +61,6 @@ private:
void _set_filter_register(void);
void _fifo_reset();
void _fifo_enable();
bool _has_auxiliary_bus();
/* Read samples from FIFO (FIFO enabled) */
@ -104,9 +103,9 @@ private:
// are we doing more than 1kHz sampling?
bool _fast_sampling;
// has master i2c been enabled?
bool _master_i2c_enable;
// Last status from register user control
uint8_t _last_stat_user_ctrl;
// buffer for fifo read
uint8_t *_fifo_buffer;

37
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -271,27 +271,18 @@ bool AP_InertialSensor_MPU9250::_init()
void AP_InertialSensor_MPU9250::_fifo_reset()
{
uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0;
uint8_t user_ctrl = _last_stat_user_ctrl;
user_ctrl &= ~(BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN);
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
_register_write(MPUREG_FIFO_EN, 0);
_register_write(MPUREG_USER_CTRL, user_ctrl);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN);
_register_write(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN);
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN, true);
hal.scheduler->delay_microseconds(1);
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
}
void AP_InertialSensor_MPU9250::_fifo_enable()
{
uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0;
_register_write(MPUREG_FIFO_EN, 0);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN);
_register_write(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN,
true);
hal.scheduler->delay(1);
_last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN;
}
bool AP_InertialSensor_MPU9250::_has_auxiliary_bus()
@ -313,7 +304,7 @@ void AP_InertialSensor_MPU9250::start()
hal.scheduler->delay(1);
// always use FIFO
_fifo_enable();
_fifo_reset();
// grab the used instances
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU9250));
@ -662,13 +653,14 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void)
// Chip reset
uint8_t tries;
for (tries = 0; tries < 5; tries++) {
uint8_t user_ctrl = _register_read(MPUREG_USER_CTRL);
_last_stat_user_ctrl = _register_read(MPUREG_USER_CTRL);
/* First disable the master I2C to avoid hanging the slaves on the
* aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus
* is used */
if (user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
_register_write(MPUREG_USER_CTRL, user_ctrl & ~BIT_USER_CTRL_I2C_MST_EN);
if (_last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
_last_stat_user_ctrl &= ~BIT_USER_CTRL_I2C_MST_EN;
_register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl);
hal.scheduler->delay(10);
}
@ -680,7 +672,8 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void)
if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
/* Disable I2C bus if SPI selected (Recommended in Datasheet to be
* done just after the device is reset) */
_register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
_last_stat_user_ctrl |= BIT_USER_CTRL_I2C_IF_DIS;
_register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl);
}
// Wake up device and select GyroZ clock. Note that the
@ -840,10 +833,10 @@ void AP_MPU9250_AuxiliaryBus::_configure_slaves()
auto &backend = AP_InertialSensor_MPU9250::from(_ins_backend);
/* Enable the I2C master to slaves on the auxiliary I2C bus*/
uint8_t user_ctrl = backend._register_read(MPUREG_USER_CTRL);
backend._register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN);
backend._master_i2c_enable = true;
if (!(backend._last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN)) {
backend._last_stat_user_ctrl |= BIT_USER_CTRL_I2C_MST_EN;
backend._register_write(MPUREG_USER_CTRL, backend._last_stat_user_ctrl);
}
/* stop condition between reads; clock at 400kHz */
backend._register_write(MPUREG_I2C_MST_CTRL,

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -62,7 +62,6 @@ private:
bool _read_sample();
void _fifo_reset();
void _fifo_enable();
/* Check if there's data available by reading register */
bool _data_ready();
@ -92,9 +91,9 @@ private:
// are we doing more than 1kHz sampling?
bool _fast_sampling;
// has master i2c been enabled?
bool _master_i2c_enable;
// Last status from register user control
uint8_t _last_stat_user_ctrl;
// buffer for fifo read
uint8_t *_fifo_buffer;