AP_InertialSensor: log unexpected register changes

when the register checking code finds an error we will log what
register changed and to what value

libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp

@@ -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);
     }
 }

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);
     }
 }

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 &reg)
+{
+    AP::logger().Write("IREG", "TimeUS,DevID,Bank,Reg,Val", "QUBBB",
+                       AP_HAL::micros64(),
+                       bus_id,
+                       reg.bank,
+                       reg.regnum,
+                       reg.value);
+}

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 &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

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);
     }

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);
     }

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);
     }

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);
     }
 }

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);
     }
 }