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

AP_InertialSensor: log unexpected register changes

Browse Source 
when the register checking code finds an error we will log what
register changed and to what value
This commit is contained in:
Andrew Tridgell 2021-02-24 10:32:39 +11:00 committed by Peter Barker
parent c23f61b79a
commit 4cddf37984
9 changed files with 39 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp
View File

@ -284,7 +284,9 @@ void AP_InertialSensor_BMI055::read_fifo_accel(void)
}
}
if (!dev_accel->check_next_register()) {
AP_HAL::Device::checkreg reg;
if (!dev_accel->check_next_register(reg)) {
log_register_change(dev_accel->get_bus_id(), reg);
_inc_accel_error_count(accel_instance);
}
}
@ -329,7 +331,9 @@ void AP_InertialSensor_BMI055::read_fifo_gyro(void)
_notify_new_gyro_raw_sample(gyro_instance, gyro);
}
if (!dev_gyro->check_next_register()) {
AP_HAL::Device::checkreg reg;
if (!dev_gyro->check_next_register(reg)) {
log_register_change(dev_gyro->get_bus_id(), reg);
_inc_gyro_error_count(gyro_instance);
}
}

2
libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
View File

@ -402,7 +402,9 @@ void AP_InertialSensor_BMI088::read_fifo_gyro(void)
_notify_new_gyro_raw_sample(gyro_instance, gyro);
}
AP_HAL::Device::checkreg reg;
if (!dev_gyro->check_next_register()) {
log_register_change(dev_gyro->get_bus_id(), reg);
_inc_gyro_error_count(gyro_instance);
}
}

10
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -621,3 +621,13 @@ bool AP_InertialSensor_Backend::should_log_imu_raw() const
return true;
}
// log an unexpected change in a register for an IMU
void AP_InertialSensor_Backend::log_register_change(uint32_t bus_id, const AP_HAL::Device::checkreg &reg)
{
AP::logger().Write("IREG", "TimeUS,DevID,Bank,Reg,Val", "QUBBB",
AP_HAL::micros64(),
bus_id,
reg.bank,
reg.regnum,
reg.value);
}

3
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -329,6 +329,9 @@ protected:
void notify_accel_fifo_reset(uint8_t instance);
void notify_gyro_fifo_reset(uint8_t instance);
// log an unexpected change in a register for an IMU
void log_register_change(uint32_t bus_id, const AP_HAL::Device::checkreg &reg);
// note that each backend is also expected to have a static detect()
// function which instantiates an instance of the backend sensor
// driver if the sensor is available

4
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -693,7 +693,9 @@ void AP_InertialSensor_Invensense::_read_fifo()
check_registers:
// check next register value for correctness
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
if (!_dev->check_next_register()) {
AP_HAL::Device::checkreg reg;
if (!_dev->check_next_register(reg)) {
log_register_change(_dev->get_bus_id(), reg);
_inc_gyro_error_count(_gyro_instance);
_inc_accel_error_count(_accel_instance);
}

4
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
View File

@ -523,7 +523,9 @@ void AP_InertialSensor_Invensensev2::_read_fifo()
check_registers:
// check next register value for correctness
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
if (!_dev->check_next_register()) {
AP_HAL::Device::checkreg reg;
if (!_dev->check_next_register(reg)) {
log_register_change(_dev->get_bus_id(), reg);
_inc_gyro_error_count(_gyro_instance);
_inc_accel_error_count(_accel_instance);
}

4
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -280,7 +280,9 @@ void AP_InertialSensor_Invensensev3::read_fifo()
check_registers:
// check next register value for correctness
dev->set_speed(AP_HAL::Device::SPEED_LOW);
if (!dev->check_next_register()) {
AP_HAL::Device::checkreg reg;
if (!dev->check_next_register(reg)) {
log_register_change(dev->get_bus_id(), reg);
_inc_gyro_error_count(gyro_instance);
_inc_accel_error_count(accel_instance);
}

7
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -695,10 +695,13 @@ void AP_InertialSensor_LSM9DS0::_poll_data()
}
// check next register value for correctness
if (!_dev_gyro->check_next_register()) {
AP_HAL::Device::checkreg reg;
if (!_dev_gyro->check_next_register(reg)) {
log_register_change(_dev_gyro->get_bus_id(), reg);
_inc_gyro_error_count(_gyro_instance);
}
if (!_dev_accel->check_next_register()) {
if (!_dev_accel->check_next_register(reg)) {
log_register_change(_dev_accel->get_bus_id(), reg);
_inc_accel_error_count(_accel_instance);
}
}

4
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp
View File

@ -426,7 +426,9 @@ void AP_InertialSensor_LSM9DS1::_poll_data()
}
// check next register value for correctness
if (!_dev->check_next_register()) {
AP_HAL::Device::checkreg reg;
if (!_dev->check_next_register(reg)) {
log_register_change(_dev->get_bus_id(), reg);
_inc_accel_error_count(_accel_instance);
}
}