From 4cddf3798419a4649a8d21b140f3f447edfa982f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 24 Feb 2021 10:32:39 +1100
Subject: [PATCH] AP_InertialSensor: log unexpected register changes
when the register checking code finds an error we will log what
register changed and to what value
---
.../AP_InertialSensor/AP_InertialSensor_BMI055.cpp | 10 +++++++---
.../AP_InertialSensor/AP_InertialSensor_BMI088.cpp | 2 ++
.../AP_InertialSensor/AP_InertialSensor_Backend.cpp | 10 ++++++++++
.../AP_InertialSensor/AP_InertialSensor_Backend.h | 3 +++
.../AP_InertialSensor/AP_InertialSensor_Invensense.cpp | 4 +++-
.../AP_InertialSensor_Invensensev2.cpp | 4 +++-
.../AP_InertialSensor_Invensensev3.cpp | 4 +++-
.../AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp | 7 +++++--
.../AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp | 4 +++-
9 files changed, 39 insertions(+), 9 deletions(-)
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);
}
}