diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp index 30c1d9329b..d3ad9e6e14 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp @@ -283,8 +283,10 @@ void AP_InertialSensor_BMI055::read_fifo_accel(void) _publish_temperature(accel_instance, temp_degc); } } - - 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); } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp index 5af1e41263..785e851ef0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp @@ -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); } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 8da87960d3..a1f2ca55e8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -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 ®) +{ + AP::logger().Write("IREG", "TimeUS,DevID,Bank,Reg,Val", "QUBBB", + AP_HAL::micros64(), + bus_id, + reg.bank, + reg.regnum, + reg.value); +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index ec283b5429..0c976d4f7f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -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 ®); + // 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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index ac7674d83e..d6ce178f56 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -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); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp index 2cc71e8038..8615b9b515 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp @@ -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); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index 7840466939..554e03c4d0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -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); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index c88f5f4ccc..fafc42283d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -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); } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp index 69fe125976..f5259a2a04 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp @@ -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); } }