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

7
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

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

2
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h
View File

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

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -669,14 +669,13 @@ void AP_InertialSensor_MPU6000::_read_fifo()
}
check_registers:
if (_reg_check_counter++ == 10) {
_reg_check_counter = 0;
// check next register value for correctness
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
if (!_dev->check_next_register()) {
_inc_gyro_error_count(_gyro_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,
@ -766,7 +765,7 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
}
// setup for register checking
_dev->setup_checked_registers(7);
_dev->setup_checked_registers(7, 20);
// initially run the bus at low speed
_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
uint8_t *_fifo_buffer;
uint8_t _reg_check_counter;
/*
accumulators for fast sampling
See description in _accumulate_fast_sampling()

8
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -605,15 +605,13 @@ bool AP_InertialSensor_MPU9250::_read_sample()
}
check_registers:
if (_reg_check_counter++ == 10) {
_reg_check_counter = 0;
// check next register value for correctness
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
if (!_dev->check_next_register()) {
_inc_gyro_error_count(_gyro_instance);
_inc_accel_error_count(_accel_instance);
}
}
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
return true;
}
@ -642,7 +640,7 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void)
}
// setup for register checking
_dev->setup_checked_registers(6);
_dev->setup_checked_registers(6, 20);
// initially run the bus at low speed
_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
uint8_t *_fifo_buffer;
uint8_t _reg_check_counter;
/*
accumulators for fast sampling
See description in _accumulate_fast_sampling()