From e4ce4ebed3bbd2aaa318fbf6a3d9a7597ac901a0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 31 Dec 2021 12:28:38 +1100 Subject: [PATCH] AP_InertialSensor: remove un-needed methods --- libraries/AP_InertialSensor/AP_InertialSensor.h | 3 --- .../AP_InertialSensor/AP_InertialSensor_Backend.cpp | 12 ------------ .../AP_InertialSensor/AP_InertialSensor_Backend.h | 6 ------ .../AP_InertialSensor/AP_InertialSensor_Logging.cpp | 4 ++-- 4 files changed, 2 insertions(+), 23 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 917ecc0d4e..eab4dc43e5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -160,9 +160,6 @@ public: const Vector3f &get_accel(uint8_t i) const { return _accel[i]; } const Vector3f &get_accel(void) const { return get_accel(_primary_accel); } - uint32_t get_gyro_error_count(uint8_t i) const { return _gyro_error_count[i]; } - uint32_t get_accel_error_count(uint8_t i) const { return _accel_error_count[i]; } - // multi-device interface bool get_gyro_health(uint8_t instance) const { return (instance<_gyro_count) ? _gyro_healthy[instance] : false; } bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index be915129a8..a4cec30818 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -664,18 +664,6 @@ void AP_InertialSensor_Backend::_set_accel_max_abs_offset(uint8_t instance, _imu._accel_max_abs_offsets[instance] = max_offset; } -// set accelerometer error_count -void AP_InertialSensor_Backend::_set_accel_error_count(uint8_t instance, uint32_t error_count) -{ - _imu._accel_error_count[instance] = error_count; -} - -// set gyro error_count -void AP_InertialSensor_Backend::_set_gyro_error_count(uint8_t instance, uint32_t error_count) -{ - _imu._gyro_error_count[instance] = error_count; -} - // increment accelerometer error_count void AP_InertialSensor_Backend::_inc_accel_error_count(uint8_t instance) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 3767c7346c..68612f575f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -234,12 +234,6 @@ protected: // publish a temperature value void _publish_temperature(uint8_t instance, float temperature); /* front end */ - // set accelerometer error_count - void _set_accel_error_count(uint8_t instance, uint32_t error_count); - - // set gyro error_count - void _set_gyro_error_count(uint8_t instance, uint32_t error_count); - // increment accelerometer error_count void _inc_accel_error_count(uint8_t instance) __RAMFUNC__; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp index 27b9a86cef..bd54f3135d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp @@ -50,8 +50,8 @@ void AP_InertialSensor::Write_IMU_instance(const uint64_t time_us, const uint8_t accel_x : accel.x, accel_y : accel.y, accel_z : accel.z, - gyro_error : get_gyro_error_count(imu_instance), - accel_error : get_accel_error_count(imu_instance), + gyro_error : _gyro_error_count[imu_instance], + accel_error : _accel_error_count[imu_instance], temperature : get_temperature(imu_instance), gyro_health : (uint8_t)get_gyro_health(imu_instance), accel_health : (uint8_t)get_accel_health(imu_instance),