mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 09:28:31 -04:00
AP_InertialSensor: log unexpected register changes
when the register checking code finds an error we will log what register changed and to what value
This commit is contained in:
parent
c23f61b79a
commit
4cddf37984
@ -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);
|
||||
}
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user