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

AP_InertialSensor: run invensese reg checking at low speed

Browse Source 
config register read/write should be at low bus speed.

also change to check every 20 calls to reduce checking cost
This commit is contained in:
Andrew Tridgell 2016-11-25 19:55:13 +11:00
parent 7100272f14
commit 27605b0258
6 changed files with 22 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -476,8 +476,8 @@ bool AP_InertialSensor_LSM9DS0::_hardware_init()
} }
// setup for register checking // setup for register checking
_dev_gyro->setup_checked_registers(5); _dev_gyro->setup_checked_registers(5, 20);
_dev_accel->setup_checked_registers(4); _dev_accel->setup_checked_registers(4, 20);
for (tries = 0; tries < 5; tries++) { for (tries = 0; tries < 5; tries++) {
_dev_gyro->set_speed(AP_HAL::Device::SPEED_LOW); _dev_gyro->set_speed(AP_HAL::Device::SPEED_LOW);
@ -694,15 +694,12 @@ bool AP_InertialSensor_LSM9DS0::_poll_data()
_read_data_transaction_a(); _read_data_transaction_a();
} }
if (_reg_check_counter++ == 10) { // check next register value for correctness
_reg_check_counter = 0; if (!_dev_gyro->check_next_register()) {
// check next register value for correctness _inc_gyro_error_count(_gyro_instance);
if (!_dev_gyro->check_next_register()) { }
_inc_gyro_error_count(_gyro_instance); if (!_dev_accel->check_next_register()) {
} _inc_accel_error_count(_accel_instance);
if (!_dev_accel->check_next_register()) {
_inc_accel_error_count(_accel_instance);
}
} }
return true; return true;

2
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h
View File

@ -103,6 +103,4 @@ private:
*/ */
enum Rotation _rotation_a; enum Rotation _rotation_a;
enum Rotation _rotation_g; enum Rotation _rotation_g;
uint8_t _reg_check_counter;
}; };

15
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -669,14 +669,13 @@ void AP_InertialSensor_MPU6000::_read_fifo()
} }
check_registers: check_registers:
if (_reg_check_counter++ == 10) { // check next register value for correctness
_reg_check_counter = 0; _dev->set_speed(AP_HAL::Device::SPEED_LOW);
// check next register value for correctness if (!_dev->check_next_register()) {
if (!_dev->check_next_register()) { _inc_gyro_error_count(_gyro_instance);
_inc_gyro_error_count(_gyro_instance); _inc_accel_error_count(_accel_instance);
_inc_accel_error_count(_accel_instance);
}
} }
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
} }
bool AP_InertialSensor_MPU6000::_block_read(uint8_t reg, uint8_t *buf, bool AP_InertialSensor_MPU6000::_block_read(uint8_t reg, uint8_t *buf,
@ -766,7 +765,7 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
} }
// setup for register checking // setup for register checking
_dev->setup_checked_registers(7); _dev->setup_checked_registers(7, 20);
// initially run the bus at low speed // initially run the bus at low speed
_dev->set_speed(AP_HAL::Device::SPEED_LOW); _dev->set_speed(AP_HAL::Device::SPEED_LOW);

2
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -109,8 +109,6 @@ private:
// buffer for fifo read // buffer for fifo read
uint8_t *_fifo_buffer; uint8_t *_fifo_buffer;
uint8_t _reg_check_counter;
/* /*
accumulators for fast sampling accumulators for fast sampling
See description in _accumulate_fast_sampling() See description in _accumulate_fast_sampling()

16
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -605,15 +605,13 @@ bool AP_InertialSensor_MPU9250::_read_sample()
} }
check_registers: check_registers:
if (_reg_check_counter++ == 10) { // check next register value for correctness
_reg_check_counter = 0; _dev->set_speed(AP_HAL::Device::SPEED_LOW);
// check next register value for correctness if (!_dev->check_next_register()) {
if (!_dev->check_next_register()) { _inc_gyro_error_count(_gyro_instance);
_inc_gyro_error_count(_gyro_instance); _inc_accel_error_count(_accel_instance);
_inc_accel_error_count(_accel_instance);
}
} }
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
return true; return true;
} }
@ -642,7 +640,7 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void)
} }
// setup for register checking // setup for register checking
_dev->setup_checked_registers(6); _dev->setup_checked_registers(6, 20);
// initially run the bus at low speed // initially run the bus at low speed
_dev->set_speed(AP_HAL::Device::SPEED_LOW); _dev->set_speed(AP_HAL::Device::SPEED_LOW);

2
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -97,8 +97,6 @@ private:
// buffer for fifo read // buffer for fifo read
uint8_t *_fifo_buffer; uint8_t *_fifo_buffer;
uint8_t _reg_check_counter;
/* /*
accumulators for fast sampling accumulators for fast sampling
See description in _accumulate_fast_sampling() See description in _accumulate_fast_sampling()