AP_InertialSensor: remove un-needed methods

This commit is contained in:
Peter Barker 2021-12-31 12:28:38 +11:00 committed by Andrew Tridgell
parent 01751fba60
commit e4ce4ebed3
4 changed files with 2 additions and 23 deletions

View File

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

View File

@ -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)
{

View File

@ -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__;

View File

@ -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),