zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: fixed handling of user_ctrl register 

with aux bus implementations
master
hiro2233 8 years ago committed by Andrew Tridgell
parent
29bbee421e
commit
cb7f46d653
4 changed files with 37 additions and 55 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 41
      libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
  2. 7
      libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
  3. 37
      libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
  4. 7
      libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h

41
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
Unescape View File

@ -321,29 +321,18 @@ bool AP_InertialSensor_MPU6000::_init()
void AP_InertialSensor_MPU6000::_fifo_reset() 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); _dev->set_speed(AP_HAL::Device::SPEED_LOW);
_register_write(MPUREG_FIFO_EN, 0); _register_write(MPUREG_FIFO_EN, 0);
_register_write(MPUREG_USER_CTRL, user_ctrl); _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_RESET);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN); _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 | _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); hal.scheduler->delay_microseconds(1);
_dev->set_speed(AP_HAL::Device::SPEED_HIGH); _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
} _last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN;
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);
} }
bool AP_InertialSensor_MPU6000::_has_auxiliary_bus() bool AP_InertialSensor_MPU6000::_has_auxiliary_bus()
@ -365,7 +354,7 @@ void AP_InertialSensor_MPU6000::start()
hal.scheduler->delay(1); hal.scheduler->delay(1);
// always use FIFO // always use FIFO
_fifo_enable(); _fifo_reset();
// grab the used instances // grab the used instances
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU6000)); _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(); _dev->get_semaphore()->give();
return false; return false;
} }
// Chip reset // Chip reset
uint8_t tries; uint8_t tries;
for (tries = 0; tries < 5; 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 /* First disable the master I2C to avoid hanging the slaves on the
* aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus * aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus
* is used */ * is used */
if (user_ctrl & BIT_USER_CTRL_I2C_MST_EN) { if (_last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
_register_write(MPUREG_USER_CTRL, 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); 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) { if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
/* Disable I2C bus if SPI selected (Recommended in Datasheet to be /* Disable I2C bus if SPI selected (Recommended in Datasheet to be
* done just after the device is reset) */ * 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 // 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); auto &backend = AP_InertialSensor_MPU6000::from(_ins_backend);
/* Enable the I2C master to slaves on the auxiliary I2C bus*/ /* Enable the I2C master to slaves on the auxiliary I2C bus*/
uint8_t user_ctrl = backend._register_read(MPUREG_USER_CTRL); if (!(backend._last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN)) {
backend._register_write(MPUREG_USER_CTRL, 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 */ /* stop condition between reads; clock at 400kHz */
backend._register_write(MPUREG_I2C_MST_CTRL, backend._register_write(MPUREG_I2C_MST_CTRL,
BIT_I2C_MST_P_NSR | BIT_I2C_MST_CLK_400KHZ); BIT_I2C_MST_P_NSR | BIT_I2C_MST_CLK_400KHZ);

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
Unescape View File

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

37
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
Unescape View File

@ -271,27 +271,18 @@ bool AP_InertialSensor_MPU9250::_init()
void AP_InertialSensor_MPU9250::_fifo_reset() 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); _dev->set_speed(AP_HAL::Device::SPEED_LOW);
_register_write(MPUREG_FIFO_EN, 0); _register_write(MPUREG_FIFO_EN, 0);
_register_write(MPUREG_USER_CTRL, user_ctrl); _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_RESET);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN); _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 | _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); hal.scheduler->delay_microseconds(1);
_dev->set_speed(AP_HAL::Device::SPEED_HIGH); _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
} _last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN;
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);
} }
bool AP_InertialSensor_MPU9250::_has_auxiliary_bus() bool AP_InertialSensor_MPU9250::_has_auxiliary_bus()
@ -313,7 +304,7 @@ void AP_InertialSensor_MPU9250::start()
hal.scheduler->delay(1); hal.scheduler->delay(1);
// always use FIFO // always use FIFO
_fifo_enable(); _fifo_reset();
// grab the used instances // grab the used instances
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU9250)); _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 // Chip reset
uint8_t tries; uint8_t tries;
for (tries = 0; tries < 5; 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 /* First disable the master I2C to avoid hanging the slaves on the
* aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus * aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus
* is used */ * is used */
if (user_ctrl & BIT_USER_CTRL_I2C_MST_EN) { if (_last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
_register_write(MPUREG_USER_CTRL, 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); 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) { if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
/* Disable I2C bus if SPI selected (Recommended in Datasheet to be /* Disable I2C bus if SPI selected (Recommended in Datasheet to be
* done just after the device is reset) */ * 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 // 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); auto &backend = AP_InertialSensor_MPU9250::from(_ins_backend);
/* Enable the I2C master to slaves on the auxiliary I2C bus*/ /* Enable the I2C master to slaves on the auxiliary I2C bus*/
uint8_t user_ctrl = backend._register_read(MPUREG_USER_CTRL); if (!(backend._last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN)) {
backend._register_write(MPUREG_USER_CTRL, 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 */ /* stop condition between reads; clock at 400kHz */
backend._register_write(MPUREG_I2C_MST_CTRL, backend._register_write(MPUREG_I2C_MST_CTRL,

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
Unescape View File

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

Write Preview
Loading…
Cancel
Save